Copter: move brake state into a structure

ArduCopter/mode.h

@@ -1040,22 +1040,24 @@ private:
     RPMode roll_mode;
     RPMode pitch_mode;
 
-    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
-
     // pilot input related variables
     float pilot_roll;                         // pilot requested roll angle (filtered to slow returns to zero)
     float pilot_pitch;                        // pilot requested roll angle (filtered to slow returns to zero)
 
     // braking related variables
-    float brake_gain;                           // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate)
-    float brake_roll;                           // target roll angle during braking periods
-    float brake_pitch;                          // target pitch angle during braking periods
-    int16_t brake_timeout_roll;                 // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
-    int16_t brake_timeout_pitch;                // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
-    float brake_angle_max_roll;                 // maximum lean angle achieved during braking.  Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
-    float brake_angle_max_pitch;                // maximum lean angle achieved during braking  Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
-    int16_t brake_to_loiter_timer;              // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER
+    struct {
+        uint8_t 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 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
+
+        float gain;                         // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from rate)
+        float roll;                         // target roll angle during braking periods
+        float pitch;                        // target pitch angle during braking periods
+        int16_t timeout_roll;               // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
+        int16_t timeout_pitch;              // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
+        float angle_max_roll;               // maximum lean angle achieved during braking.  Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
+        float angle_max_pitch;              // maximum lean angle achieved during braking  Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
+        int16_t to_loiter_timer;            // cycles to mix brake and loiter controls in POSHOLD_TO_LOITER
+    } brake;
 
     // loiter related variables
     int16_t controller_to_pilot_timer_roll;     // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT

ArduCopter/mode_poshold.cpp

@@ -42,7 +42,7 @@ bool ModePosHold::init(bool ignore_checks)
     pilot_pitch = 0.0f;
 
     // compute brake_gain
-    brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f;
+    brake.gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f;
 
     if (copter.ap.land_complete) {
         // if landed begin in loiter mode
@@ -69,7 +69,6 @@ bool ModePosHold::init(bool ignore_checks)
 void ModePosHold::run()
 {
     float takeoff_climb_rate = 0.0f;
-    float brake_to_loiter_mix;          // mix of brake and loiter controls.  0 = fully brake controls, 1 = fully loiter controls
     float controller_to_pilot_roll_mix; // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
     float controller_to_pilot_pitch_mix;    // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
     const Vector3f& vel = inertial_nav.get_velocity();
@@ -206,10 +205,10 @@ void ModePosHold::run()
             if (is_zero(target_roll) && (fabsf(pilot_roll) < 2 * g.poshold_brake_rate)) {
                 // initialise BRAKE mode
                 roll_mode = RPMode::BRAKE;        // Set brake roll mode
-                brake_roll = 0.0f;                  // initialise braking angle to zero
-                brake_angle_max_roll = 0.0f;        // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
-                brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
-                braking_time_updated_roll = false;   // flag the braking time can be re-estimated
+                brake.roll = 0.0f;                  // initialise braking angle to zero
+                brake.angle_max_roll = 0.0f;        // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
+                brake.timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
+                brake.time_updated_roll = false;   // flag the braking time can be re-estimated
             }
 
             // final lean angle should be pilot input plus wind compensation
@@ -218,29 +217,29 @@ void ModePosHold::run()
 
         case RPMode::BRAKE:
         case RPMode::BRAKE_READY_TO_LOITER:
-            // calculate brake_roll angle to counter-act velocity
-            update_brake_angle_from_velocity(brake_roll, vel_right);
+            // calculate brake.roll angle to counter-act velocity
+            update_brake_angle_from_velocity(brake.roll, vel_right);
 
             // update braking time estimate
-            if (!braking_time_updated_roll) {
+            if (!brake.time_updated_roll) {
                 // check if brake angle is increasing
-                if (fabsf(brake_roll) >= brake_angle_max_roll) {
-                    brake_angle_max_roll = fabsf(brake_roll);
+                if (fabsf(brake.roll) >= brake.angle_max_roll) {
+                    brake.angle_max_roll = fabsf(brake.roll);
                 } else {
                     // braking angle has started decreasing so re-estimate braking time
-                    brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake_roll))/(10L*(int32_t)g.poshold_brake_rate));  // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
-                    braking_time_updated_roll = true;
+                    brake.timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake.roll))/(10L*(int32_t)g.poshold_brake_rate));  // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
+                    brake.time_updated_roll = true;
                 }
             }
 
             // if velocity is very low reduce braking time to 0.5seconds
-            if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (brake_timeout_roll > 50*LOOP_RATE_FACTOR)) {
-                brake_timeout_roll = 50*LOOP_RATE_FACTOR;
+            if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (brake.timeout_roll > 50*LOOP_RATE_FACTOR)) {
+                brake.timeout_roll = 50*LOOP_RATE_FACTOR;
             }
 
             // reduce braking timer
-            if (brake_timeout_roll > 0) {
-                brake_timeout_roll--;
+            if (brake.timeout_roll > 0) {
+                brake.timeout_roll--;
             } else {
                 // indicate that we are ready to move to Loiter.
                 // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
@@ -249,7 +248,7 @@ void ModePosHold::run()
             }
 
             // final lean angle is braking angle + wind compensation angle
-            roll = brake_roll + wind_comp_roll;
+            roll = brake.roll + wind_comp_roll;
 
             // check for pilot input
             if (!is_zero(target_roll)) {
@@ -300,10 +299,10 @@ void ModePosHold::run()
             if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) {
                 // initialise BRAKE mode
                 pitch_mode = RPMode::BRAKE;       // set brake pitch mode
-                brake_pitch = 0.0f;                 // initialise braking angle to zero
-                brake_angle_max_pitch = 0.0f;       // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
-                brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
-                braking_time_updated_pitch = false;   // flag the braking time can be re-estimated
+                brake.pitch = 0.0f;                 // initialise braking angle to zero
+                brake.angle_max_pitch = 0.0f;       // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
+                brake.timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
+                brake.time_updated_pitch = false;   // flag the braking time can be re-estimated
             }
 
             // final lean angle should be pilot input plus wind compensation
@@ -313,28 +312,28 @@ void ModePosHold::run()
         case RPMode::BRAKE:
         case RPMode::BRAKE_READY_TO_LOITER:
             // calculate brake_pitch angle to counter-act velocity
-            update_brake_angle_from_velocity(brake_pitch, -vel_fw);
+            update_brake_angle_from_velocity(brake.pitch, -vel_fw);
 
             // update braking time estimate
-            if (!braking_time_updated_pitch) {
+            if (!brake.time_updated_pitch) {
                 // check if brake angle is increasing
-                if (fabsf(brake_pitch) >= brake_angle_max_pitch) {
-                    brake_angle_max_pitch = fabsf(brake_pitch);
+                if (fabsf(brake.pitch) >= brake.angle_max_pitch) {
+                    brake.angle_max_pitch = fabsf(brake.pitch);
                 } else {
                     // braking angle has started decreasing so re-estimate braking time
-                    brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake_pitch))/(10L*(int32_t)g.poshold_brake_rate));  // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
-                    braking_time_updated_pitch = true;
+                    brake.timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake.pitch))/(10L*(int32_t)g.poshold_brake_rate));  // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
+                    brake.time_updated_pitch = true;
                 }
             }
 
             // if velocity is very low reduce braking time to 0.5seconds
-            if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) {
-                brake_timeout_pitch = 50*LOOP_RATE_FACTOR;
+            if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (brake.timeout_pitch > 50*LOOP_RATE_FACTOR)) {
+                brake.timeout_pitch = 50*LOOP_RATE_FACTOR;
             }
 
             // reduce braking timer
-            if (brake_timeout_pitch > 0) {
-                brake_timeout_pitch--;
+            if (brake.timeout_pitch > 0) {
+                brake.timeout_pitch--;
             } else {
                 // indicate that we are ready to move to Loiter.
                 // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
@@ -343,7 +342,7 @@ void ModePosHold::run()
             }
 
             // final lean angle is braking angle + wind compensation angle
-            pitch = brake_pitch + wind_comp_pitch;
+            pitch = brake.pitch + wind_comp_pitch;
 
             // check for pilot input
             if (!is_zero(target_pitch)) {
@@ -386,7 +385,7 @@ void ModePosHold::run()
     if (roll_mode == RPMode::BRAKE_READY_TO_LOITER && pitch_mode == RPMode::BRAKE_READY_TO_LOITER) {
         roll_mode = RPMode::BRAKE_TO_LOITER;
         pitch_mode = RPMode::BRAKE_TO_LOITER;
-        brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
+        brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
         // init loiter controller
         loiter_nav->init_target(inertial_nav.get_position());
         // set delay to start of wind compensation estimate updates
@@ -401,29 +400,30 @@ void ModePosHold::run()
 
         // handle combined roll+pitch mode
         switch (roll_mode) {
-            case RPMode::BRAKE_TO_LOITER:
+            case RPMode::BRAKE_TO_LOITER: {
                 // reduce brake_to_loiter timer
-                if (brake_to_loiter_timer > 0) {
-                    brake_to_loiter_timer--;
+                if (brake.to_loiter_timer > 0) {
+                    brake.to_loiter_timer--;
                 } else {
                     // progress to full loiter on next iteration
                     roll_mode = RPMode::LOITER;
                     pitch_mode = RPMode::LOITER;
                 }
 
-                // calculate percentage mix of loiter and brake control
-                brake_to_loiter_mix = (float)brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
+                // mix of brake and loiter controls.  0 = fully brake
+                // controls, 1 = fully loiter controls
+                const float brake_to_loiter_mix = (float)brake.to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
 
-                // calculate brake_roll and pitch angles to counter-act velocity
-                update_brake_angle_from_velocity(brake_roll, vel_right);
-                update_brake_angle_from_velocity(brake_pitch, -vel_fw);
+                // calculate brake.roll and pitch angles to counter-act velocity
+                update_brake_angle_from_velocity(brake.roll, vel_right);
+                update_brake_angle_from_velocity(brake.pitch, -vel_fw);
 
                 // run loiter controller
                 loiter_nav->update(false);
 
                 // calculate final roll and pitch output by mixing loiter and brake controls
-                roll = mix_controls(brake_to_loiter_mix, brake_roll + wind_comp_roll, loiter_nav->get_roll());
-                pitch = mix_controls(brake_to_loiter_mix, brake_pitch + wind_comp_pitch, loiter_nav->get_pitch());
+                roll = mix_controls(brake_to_loiter_mix, brake.roll + wind_comp_roll, loiter_nav->get_roll());
+                pitch = mix_controls(brake_to_loiter_mix, brake.pitch + wind_comp_pitch, loiter_nav->get_pitch());
 
                 // check for pilot input
                 if (!is_zero(target_roll) || !is_zero(target_pitch)) {
@@ -432,7 +432,7 @@ void ModePosHold::run()
                         // init transition to pilot override
                         roll_controller_to_pilot_override();
                         // switch pitch-mode to brake (but ready to go back to loiter anytime)
-                        // no need to reset brake_pitch here as wind comp has not been updated since last brake_pitch computation
+                        // no need to reset brake.pitch here as wind comp has not been updated since last brake.pitch computation
                         pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
                     }
                     // if pitch input switch to pilot override for pitch
@@ -441,13 +441,13 @@ void ModePosHold::run()
                         pitch_controller_to_pilot_override();
                         if (is_zero(target_roll)) {
                             // switch roll-mode to brake (but ready to go back to loiter anytime)
-                            // no need to reset brake_roll here as wind comp has not been updated since last brake_roll computation
+                            // no need to reset brake.roll here as wind comp has not been updated since last brake.roll computation
                             roll_mode = RPMode::BRAKE_READY_TO_LOITER;
                         }
                     }
                 }
                 break;
-
+            }
             case RPMode::LOITER:
                 // run loiter controller
                 loiter_nav->update(false);
@@ -467,8 +467,8 @@ void ModePosHold::run()
                         roll_controller_to_pilot_override();
                         // switch pitch-mode to brake (but ready to go back to loiter anytime)
                         pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
-                        // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
-                        brake_pitch = 0.0f;
+                        // reset brake.pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
+                        brake.pitch = 0.0f;
                     }
                     // if pitch input switch to pilot override for pitch
                     if (!is_zero(target_pitch)) {
@@ -477,7 +477,7 @@ void ModePosHold::run()
                         // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time)
                         if (is_zero(target_roll)) {
                             roll_mode = RPMode::BRAKE_READY_TO_LOITER;
-                            brake_roll = 0.0f;
+                            brake.roll = 0.0f;
                         }
                             // if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
                     }
@@ -546,9 +546,9 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel
 
     // calculate velocity-only based lean angle
     if (velocity >= 0) {
-        lean_angle = -brake_gain * velocity * (1.0f + 500.0f / (velocity + 60.0f));
+        lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f));
     } else {
-        lean_angle = -brake_gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f));
+        lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f));
     }
 
     // do not let lean_angle be too far from brake_angle