I have been putting a Trex500 together with an APM 2.5 with AC 3.0.1. I have configured the tail as “servo only”, so no external gyro. The manual says “Next rotate the helicopter slowly. You should find that in a certain direction the rudder stays in the middle. This is the APM trying to hold that heading.” This sounds like normal heading hold gyro operation. However, while testing on the bench in stabilize mode I noticed that the servo only responds like a rate gyro (always returns to zero when movement stops no matter what the heading.)
I have checked that the STAB-YAW-P term is not zero. I tried flying it on Sunday thinking the mode might change once flying, but it flew like a rate mode. I had a slow drift in yaw so probably need to mechanically adjust the tail linkage, but still consistent with a rate mode. After coming home I tried loading 3.1 and it behaved the same on the bench.
Should I be expecting a heading hold mode, or is the rate gyro behavior I observe correct? If it is correct is there another mode that does do heading hold?
Thank you,
Andrew
Hmmm… Can you send me your parameter file?
It definitely should be operating as a heading hold. There is no pure rate mode, even in Acro it is acting as HH. I can’t even figure out how what parameters could be set wrong that would make it act like pure rate.
Do you have a compass installed?
If you had a steady drift, then it must be because of a trim problem on the yaw RC input.
Hi Rob,
Yes, I have a compass installed and it appears to be working - values make sense when I rotate the heli, and if I wave a magnet in the general area the values change.
A copy of my parameter file and log file are attached. I think that the parameter file was saved after I updated from 3.0.1 to 3.1, but the log file is from 3.0.1.
I wondered about the rudder input, but it seems to go back to zero in this extract from the log file. You can see the tail keep drifting and then I bring it back straight again.
[attachment=0]Yaw Drift.jpg[/attachment]
I would appreciate any suggestions you might have,
Thank you,
Andrew
So that log is in-flight?
Ok, I had a look. I’m pretty sure that the issue is the Rate_Yaw_I is much too low at 0.012. I can’t remember off the top of my head what value I have, but IIRC, it should be closer to the value of your Rate_Yaw_P. So try increasing it to 0.1, then try 0.2. You might even be able to go higher.
I also think the Rate_Yaw_Imax is too low at 800. If your rudder linkage is perfectly set up, this does not need to be high. But if it’s not set up perfectly, you will get the effect you’re seeing. I would suggest to increase this to 1500, even 2500. It’s not going to use it if it doesn’t need it, so there’s little risk here.
The way I want to structure the code, is to not require people to need to worry about mechanical trimming on the rudder. I want it where, you set up the linkage such that the servo arm is centred with the HS4 trim. And the blade pitch is 0° at neutral. Then have the actual offset that use used to put in mechanically, done in the code. This is already somewhat done by ColYaw. But it still needs a minimum offset any time the motor is switched on, enough to resist motor torque at 0° collective.
The system is too intelligent to continue requiring people to adjust linkages beyond a basic physical setup.
In the meantime, Rate_Yaw_I term takes care of this. You’ll usually get a little tail drift on your first liftoff, and that’s it, then it’s locked in for the rest of the flight. The large Rate Yaw I is like an auto-trim and/or auto-linkage-adjuster. Your problem is the value just isn’t big enough.
I changed the values as suggested, but it still seems to behave the same way on the bench. I had a look at the code, and saw the following routine. Is this the yaw controller active in stabilize mode? If so, then it looks like the angle error is kept at zero until the motor run-up is complete, in which case I need to get it flying before I draw any conclusions about the new values.
get_yaw_rate_stabilized_ef(int32_t stick_angle)
{
int32_t angle_error = 0;
// convert the input to the desired yaw rate
int32_t target_rate = stick_angle * g.acro_yaw_p;
// convert the input to the desired yaw rate
nav_yaw += target_rate * G_Dt;
nav_yaw = wrap_360_cd(nav_yaw);
// calculate difference between desired heading and current heading
angle_error = wrap_180_cd(nav_yaw - ahrs.yaw_sensor);
// limit the maximum overshoot
angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
#if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete()) {
angle_error = 0;
}
#else
// reset target angle to current heading if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
#endif // HELI_FRAME
// update nav_yaw to be within max_angle_overshoot of our current heading
nav_yaw = wrap_360_cd(angle_error + ahrs.yaw_sensor);
// set earth frame targets for rate controller
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
}
Yes, that’s the relevant section of code.
It’s always difficult to bench test this type of stuff that actually relies on having the thing flying because the code is written with dynamics in mind.
But, you should be able to bench test this by just faking the motor run-up. Just disconnect the ESC.
Tried it with the ESC unplugged and it seems fine with the new gains - tail responds like a heading hold gyro.
Next thing is to try a flight whenever the weather co-operates. If it continues like it has been lately it could be a while.
Thank you for the help,
Andrew
Andrew,
I’m having trouble with configuring the Mission Planner for Heading Hold using a traditional copter frame.
I know its an old post so not sure if you are still flying but wondered if you could send me a Config file to load into my Mission Planner to bench test the rudder settings for heading hold.
Any help would be gladly received.
Many thanks,
Gabriel