Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Hybrid move reset_I into structure

Browse Source 
Saves 1 byte of RAM
This commit is contained in:
Randy Mackay 2014-04-23 14:38:06 +09:00
parent 4d5b73b968
commit ba94fc9796
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/control_hybrid.pde
View File

@ -56,6 +56,7 @@ static struct {
hybrid_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter hybrid_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter
uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t loiter_reset_I : 1; // true the very first time hybrid enters loiter, thereafter we trust the i terms loiter has
// pilot input related variables // pilot input related variables
int16_t pilot_roll; // pilot requested roll angle (filtered to slow returns to zero) int16_t pilot_roll; // pilot requested roll angle (filtered to slow returns to zero)
@ -90,8 +91,6 @@ static struct {
int16_t pitch; // final pitch angle sent to attitude controller int16_t pitch; // final pitch angle sent to attitude controller
} hybrid; } hybrid;
static bool reset_I = true;
// hybrid_init - initialise hybrid controller // hybrid_init - initialise hybrid controller
static bool hybrid_init(bool ignore_checks) static bool hybrid_init(bool ignore_checks)
{ {
@ -127,6 +126,9 @@ static bool hybrid_init(bool ignore_checks)
hybrid.pitch_mode = HYBRID_PILOT_OVERRIDE; hybrid.pitch_mode = HYBRID_PILOT_OVERRIDE;
} }
// loiter's I terms should be reset the first time only
hybrid.loiter_reset_I = true;
// initialise wind_comp each time hybrid is switched on // initialise wind_comp each time hybrid is switched on
hybrid.wind_comp_ef.zero(); hybrid.wind_comp_ef.zero();
hybrid.wind_comp_roll = 0; hybrid.wind_comp_roll = 0;
@ -400,9 +402,9 @@ static void hybrid_run()
// init loiter controller // init loiter controller
Vector3f curr_pos = inertial_nav.get_position(); 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 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)); wp_nav.set_loiter_target(curr_pos, hybrid.loiter_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 // 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; hybrid.loiter_reset_I = false;
// set delay to start of wind compensation estimate updates // set delay to start of wind compensation estimate updates
hybrid.wind_comp_start_timer = HYBRID_WIND_COMP_START_TIMER; hybrid.wind_comp_start_timer = HYBRID_WIND_COMP_START_TIMER;
} }