Copter: Hybrid fixes to wind_comp, brake pitch timer, thr peaks
There was an error in the velocity axis used to update
brake_timeout_pitch (vel_right instead of vel_fw)
The wind_comp was not enough filtered for the Pixhawk (400Hz), so I
added a specific time constant (TC_WIND_COMP) to have the expected
filter with 400Hz controllers.
About throttle peaks, after some tests and from logs, they happen when
hybrid switches to loiter.
There is always a difference between Alt and DesiredAlt (DAlt), but,
when loiter engages, it initializes DAlt = Alt and the copter tries
immediatelly to reach that new setpoint. So the solution would be to
init_loiter_target() just as it was in pre-onion code : only x/y and not
z. and to be able to pass parameters like that
wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
Well, from this new code structure, it seems not possible with current
functions so I've used set_loiter_target that init position passed as
parameter and velocity to 0 (as expected).
BTW, I think there was something wrong with set_loiter_target function,
the "Vector3f& position" parameter was not used at all...
I moved the reset flag from init_loiter_target to set_loiter_target.
# define HYBRID_BRAKE_TIME_ESTIMATE_MAX 600 // max number of cycles the brake will be applied before we switch to loiter
# define HYBRID_BRAKE_TO_LOITER_TIMER 150 // Number of cycles to transition from brake mode to loiter mode.
# define HYBRID_WIND_COMP_START_TIMER 150 // Number of cycles to start wind compensation update after loiter is engaged
# define HYBRID_CONTROLLER_TO_PILOT_MIX_TIMER 50 // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
# define HYBRID_SMOOTH_RATE_FACTOR 0.05f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low.
# define HYBRID_WIND_COMP_TIMER_10HZ 10 // counter value used to reduce wind compensation to 10hz
# define LOOP_RATE_FACTOR 1 // used to adapt hybrid params to loop_rate
# define TC_WIND_COMP 0.01f // Time constant for hybrid_update_wind_comp_estimate()
#else
// definitions for 400hz loop update rate
# define HYBRID_BRAKE_TIME_ESTIMATE_MAX (600*4) // max number of cycles the brake will be applied before we switch to loiter
# define HYBRID_BRAKE_TO_LOITER_TIMER (150*4) // Number of cycles to transition from brake mode to loiter mode. Must be lower than HYBRID_LOITER_STAB_TIMER
# define HYBRID_WIND_COMP_START_TIMER (150*4) // Number of cycles to start wind compensation update after loiter is engaged
# define HYBRID_CONTROLLER_TO_PILOT_MIX_TIMER (50*4) // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
# define HYBRID_SMOOTH_RATE_FACTOR 0.0125f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low.
# define HYBRID_WIND_COMP_TIMER_10HZ 40 // counter value used to reduce wind compensation to 10hz
# define LOOP_RATE_FACTOR 4 // used to adapt hybrid params to loop_rate
# define TC_WIND_COMP 0.0025f // Time constant for hybrid_update_wind_comp_estimate()
#endif
// definitions that are independent of main loop rate
// only init here as we can switch to hybrid in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
wp_nav.init_loiter_target();
}else{
// if not landed start in pilot override to avoid hard twitch
wp_nav.init_loiter_target(false); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
Vector3f curr_pos = inertial_nav.get_position();
curr_pos.z = pos_control.get_alt_target(); // We don't set alt_target to current altitude but to the current alt_target... the easiest would be to set only x/y as it was on pre-onion code
wp_nav.set_loiter_target(curr_pos, reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
// at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore
reset_I = false;
// set delay to start of wind compensation estimate updates
// switch pitch-mode to brake (but ready to go back to loiter anytime)
hybrid.pitch_mode = HYBRID_BRAKE_READY_TO_LOITER; // JD : no need to reset hybrid.brake_pitch here as wind comp has not been updated since last brake_pitch computation
// no need to reset hybrid.brake_pitch here as wind comp has not been updated since last brake_pitch computation
hybrid.pitch_mode = HYBRID_BRAKE_READY_TO_LOITER;
}
// if pitch input switch to pilot override for pitch
// switch roll-mode to brake (but ready to go back to loiter anytime)
hybrid.roll_mode = HYBRID_BRAKE_READY_TO_LOITER; // JD : no need to reset hybrid.brake_roll here as wind comp has not been updated since last brake_roll computation
// no need to reset hybrid.brake_roll here as wind comp has not been updated since last brake_roll computation
// switch pitch-mode to brake (but ready to go back to loiter anytime)
hybrid.pitch_mode = HYBRID_BRAKE_READY_TO_LOITER;
hybrid.brake_pitch = 0; // JD : reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
// reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
hybrid.brake_pitch = 0;
}
// if pitch input switch to pilot override for pitch
Ju1ien
11 years ago
committed by
Randy Mackay