Blimp: Code cleanups

Blimp/AP_Arming.cpp

@@ -40,14 +40,6 @@ bool AP_Arming_Blimp::run_pre_arm_checks(bool display_failure)
         return false;
     }
 
-    // check if motor interlock aux switch is in use
-    // if it is, switch needs to be in disabled position to arm
-    // otherwise exit immediately.  This check to be repeated,
-    // as state can change at any time.
-    if (blimp.ap.using_interlock && blimp.ap.motor_interlock_switch) {
-        check_failed(display_failure, "Motor Interlock Enabled");
-    }
-
     // if pre arm checks are disabled run only the mandatory checks
     if (checks_to_perform == 0) {
         return mandatory_checks(display_failure);
@@ -273,6 +265,15 @@ bool AP_Arming_Blimp::mandatory_gps_checks(bool display_failure)
         return true;
     }
 
+    // check for GPS glitch (as reported by EKF)
+    nav_filter_status filt_status;
+    if (ahrs.get_filter_status(filt_status)) {
+        if (filt_status.flags.gps_glitching) {
+            check_failed(display_failure, "GPS glitching");
+            return false;
+        }
+    }
+
     // if we got here all must be ok
     return true;
 }
@@ -383,7 +384,7 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c
         blimp.arming_altitude_m = blimp.inertial_nav.get_altitude() * 0.01;
     }
 
-    // enable gps velocity based centrefugal force compensation
+    // enable gps velocity based centrifugal force compensation
     ahrs.set_correct_centrifugal(true);
     hal.util->set_soft_armed(true);
 

Blimp/Blimp.cpp

@@ -142,7 +142,7 @@ void Blimp::update_batt_compass(void)
 // Full rate logging of attitude, rate and pid loops
 void Blimp::full_rate_logging()
 {
-    if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) {
+    if (should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
     }
     if (should_log(MASK_LOG_PID)) {
@@ -155,7 +155,7 @@ void Blimp::full_rate_logging()
 void Blimp::ten_hz_logging_loop()
 {
     // log attitude data if we're not already logging at the higher rate
-    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) {
+    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
     }
     // log EKF attitude data
@@ -249,20 +249,20 @@ void Blimp::update_altitude()
 }
 
 //Conversions are in 2D so that up remains up in world frame when the blimp is not exactly level.
-void Blimp::rotate_BF_to_NE(float &x, float &y)
+void Blimp::rotate_BF_to_NE(Vector2f &vec)
 {
-    float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
+    float ne_x = vec.x*ahrs.cos_yaw() - vec.y*ahrs.sin_yaw();
-    float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
+    float ne_y = vec.x*ahrs.sin_yaw() + vec.y*ahrs.cos_yaw();
-    x = ne_x;
+    vec.x = ne_x;
-    y = ne_y;
+    vec.y = ne_y;
 }
 
-void Blimp::rotate_NE_to_BF(float &x, float &y)
+void Blimp::rotate_NE_to_BF(Vector2f &vec)
 {
-    float bf_x = x*ahrs.cos_yaw() + y*ahrs.sin_yaw();
+    float bf_x = vec.x*ahrs.cos_yaw() + vec.y*ahrs.sin_yaw();
-    float bf_y = -x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
+    float bf_y = -vec.x*ahrs.sin_yaw() + vec.y*ahrs.cos_yaw();
-    x = bf_x;
+    vec.x = bf_x;
-    y = bf_y;
+    vec.y = bf_y;
 
 }
 

Blimp/Blimp.h

@@ -150,35 +150,26 @@ private:
     // Documentation of Globals:
     typedef union {
         struct {
-            uint8_t unused1                 : 1; // 0
+            uint8_t pre_arm_rc_check        : 1; // 1       // true if rc input pre-arm checks have been completed successfully
-            uint8_t pre_arm_rc_check        : 1; // 3       // true if rc input pre-arm checks have been completed successfully
+            uint8_t pre_arm_check           : 1; // 2       // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
-            uint8_t pre_arm_check           : 1; // 4       // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
+            uint8_t auto_armed              : 1; // 3       // stops auto missions from beginning until throttle is raised
-            uint8_t auto_armed              : 1; // 5       // stops auto missions from beginning until throttle is raised
+            uint8_t logging_started         : 1; // 4       // true if logging has started
-            uint8_t logging_started         : 1; // 6       // true if logging has started
+            uint8_t land_complete           : 1; // 5       // true if we have detected a landing
-            uint8_t land_complete           : 1; // 7       // true if we have detected a landing
+            uint8_t new_radio_frame         : 1; // 6       // Set true if we have new PWM data to act on from the Radio
-            uint8_t new_radio_frame         : 1; // 8       // Set true if we have new PWM data to act on from the Radio
+            uint8_t rc_receiver_present     : 1; // 7       // true if we have an rc receiver present (i.e. if we've ever received an update
-            uint8_t usb_connected_unused    : 1; // 9       // UNUSED
+            uint8_t compass_mot             : 1; // 8       // true if we are currently performing compassmot calibration
-            uint8_t rc_receiver_present     : 1; // 10      // true if we have an rc receiver present (i.e. if we've ever received an update
+            uint8_t motor_test              : 1; // 9       // true if we are currently performing the motors test
-            uint8_t compass_mot             : 1; // 11      // true if we are currently performing compassmot calibration
+            uint8_t initialised             : 1; // 10      // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
-            uint8_t motor_test              : 1; // 12      // true if we are currently performing the motors test
+            uint8_t land_complete_maybe     : 1; // 11      // true if we may have landed (less strict version of land_complete)
-            uint8_t initialised             : 1; // 13      // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
+            uint8_t throttle_zero           : 1; // 12      // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down
-            uint8_t land_complete_maybe     : 1; // 14      // true if we may have landed (less strict version of land_complete)
+            uint8_t gps_glitching           : 1; // 13      // true if GPS glitching is affecting navigation accuracy
-            uint8_t throttle_zero           : 1; // 15      // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
+            uint8_t in_arming_delay         : 1; // 14      // true while we are armed but waiting to spin motors
-            uint8_t system_time_set_unused  : 1; // 16      // true if the system time has been set from the GPS
+            uint8_t initialised_params      : 1; // 15      // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
-            uint8_t gps_glitching           : 1; // 17      // true if GPS glitching is affecting navigation accuracy
-            uint8_t using_interlock         : 1; // 20      // aux switch motor interlock function is in use
-            uint8_t land_repo_active        : 1; // 21      // true if the pilot is overriding the landing position
-            uint8_t motor_interlock_switch  : 1; // 22      // true if pilot is requesting motor interlock enable
-            uint8_t in_arming_delay         : 1; // 23      // true while we are armed but waiting to spin motors
-            uint8_t initialised_params      : 1; // 24      // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
-            uint8_t unused3                 : 1; // 25      // was compass_init_location; true when the compass's initial location has been set
-            uint8_t unused2                 : 1; // 26      // aux switch rc_override is allowed
-            uint8_t unused4                 : 1; // 27      // was "we armed using a arming switch"
         };
         uint32_t value;
     } ap_t;
 
-    ap_t ap; //MIR Set of general variables
+    ap_t ap;
 
     static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t");
 
@@ -330,8 +321,8 @@ private:
     void one_hz_loop();
     void read_AHRS(void);
     void update_altitude();
-    void rotate_NE_to_BF(float &x, float &y);
+    void rotate_NE_to_BF(Vector2f &vec);
-    void rotate_BF_to_NE(float &x, float &y);
+    void rotate_BF_to_NE(Vector2f &vec);
 
     // commands.cpp
     void update_home_from_EKF();

Blimp/Fins.h

@@ -96,6 +96,8 @@ public:
     float get_throttle()
     {
         //Only for Mavlink - essentially just an indicator of how hard the fins are working.
+        //Note that this is the unconstrained version, so if the higher level control gives too high input, 
+        //throttle will be displayed as more than 100.
         return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out)));
     }
 

Blimp/Parameters.cpp

@@ -252,42 +252,42 @@ const AP_Param::Info Blimp::var_info[] = {
     // @Param: MAX_VEL_XY
     // @DisplayName: Max XY Velocity
     // @Description: Sets the maximum XY velocity, in m/s
-    // @Values: 
+    // @Range: 0.2 5
     // @User: Standard
     GSCALAR(max_vel_xy, "MAX_VEL_XY", 0.5),
 
     // @Param: MAX_VEL_Z
     // @DisplayName: Max Z Velocity
     // @Description: Sets the maximum Z velocity, in m/s
-    // @Values: 
+    // @Range: 0.2 5
     // @User: Standard
     GSCALAR(max_vel_z, "MAX_VEL_Z", 0.4),
 
     // @Param: MAX_VEL_YAW
     // @DisplayName: Max yaw Velocity
     // @Description: Sets the maximum yaw velocity, in rad/s
-    // @Values: 
+    // @Range: 0.2 5
     // @User: Standard
     GSCALAR(max_vel_yaw, "MAX_VEL_YAW", 0.5),
 
     // @Param: MAX_POS_XY
     // @DisplayName: Max XY Position change
     // @Description: Sets the maximum XY position change, in m/s
-    // @Values: 
+    // @Range: 0.1 5
     // @User: Standard
     GSCALAR(max_pos_xy, "MAX_POS_XY", 0.2),
 
     // @Param: MAX_POS_Z
     // @DisplayName: Max Z Position change
     // @Description: Sets the maximum Z position change, in m/s
-    // @Values: 
+    // @Range: 0.1 5
     // @User: Standard
     GSCALAR(max_pos_z, "MAX_POS_Z", 0.15),
 
     // @Param: MAX_POS_YAW
     // @DisplayName: Max Yaw Position change
     // @Description: Sets the maximum Yaw position change, in rad/s
-    // @Values: 
+    // @Range: 0.1 5
     // @User: Standard
     GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3),
 
@@ -306,9 +306,6 @@ const AP_Param::Info Blimp::var_info[] = {
     // @User: Standard
     GSCALAR(dis_mask, "DIS_MASK", 0),
 
-    GSCALAR(notch_bw, "NOTCH_BW", 2),
-    GSCALAR(notch_att, "NOTCH_ATT", 15),
-
     // @Param: RC_SPEED
     // @DisplayName: ESC Update Speed
     // @Description: This is the speed in Hertz that your ESCs will receive updates

Blimp/Parameters.h

@@ -67,10 +67,10 @@ public:
         k_param_parachute,
 
         // Landing gear object
-        k_param_landinggear,    // 18
+        k_param_landinggear,
 
         // Input Management object
-        k_param_input_manager,  // 19
+        k_param_input_manager,
 
         // Misc
         k_param_gps_hdop_good,
@@ -84,19 +84,30 @@ public:
         k_param_log_bitmask,
         k_param_throttle_filt,
         k_param_throttle_behavior,
-        k_param_pilot_takeoff_alt, // 64
+        k_param_pilot_takeoff_alt,
 
         // AP_ADSB Library
-        k_param_adsb,                   // 72
+        k_param_adsb,
-        k_param_notify,                 // 73
+        k_param_notify,
 
         //PID Controllers
-        k_param_pid_vel_xy,
+        k_param_pid_vel_xy = 32,
-        k_param_pid_pos_xy,
         k_param_pid_vel_z,
         k_param_pid_vel_yaw,
+        k_param_pid_pos_xy,
         k_param_pid_pos_z,
         k_param_pid_pos_yaw,
+
+        //Position & Velocity controller params
+        k_param_max_vel_xy = 50,
+        k_param_max_vel_z,
+        k_param_max_vel_yaw,
+        k_param_max_pos_xy,
+        k_param_max_pos_z,
+        k_param_max_pos_yaw,
+        k_param_simple_mode,
+        k_param_dis_mask,
+
         //
         // 90: misc2
         //
@@ -180,17 +191,7 @@ public:
         // 220: Misc
         //
         k_param_fs_ekf_action = 220,
-        k_param_max_vel_xy,
         k_param_arming,
-        k_param_max_vel_z, 
-        k_param_simple_mode,
-        k_param_dis_mask, //225
-        k_param_notch_bw,
-        k_param_notch_att,
-        k_param_max_vel_yaw,
-        k_param_max_pos_xy,
-        k_param_max_pos_z,
-        k_param_max_pos_yaw,
 
         k_param_logger = 253, // 253 - Logging Group
 
@@ -252,8 +253,6 @@ public:
 
     AP_Int8         simple_mode;
     AP_Int16        dis_mask;
-    AP_Float        notch_bw;
-    AP_Float        notch_att;
 
     AP_Int8         rtl_alt_type;
 

Blimp/mode.h

@@ -47,10 +47,6 @@ public:
     {
         return false;
     }
-    virtual bool logs_attitude() const
-    {
-        return false;
-    }
 
     // return a string for this flightmode
     virtual const char *name() const = 0;

Blimp/mode_loiter.cpp

@@ -14,18 +14,19 @@
 //Runs the main loiter controller
 void ModeLoiter::run()
 {
-    float pilot_fwd = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
+    Vector3f pilot;
-    float pilot_rgt = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
+    pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
-    float pilot_dwn = channel_down->get_control_in()  / float(RC_SCALE) * g.max_pos_z * blimp.scheduler.get_loop_period_s();
+    pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
+    pilot.z = channel_down->get_control_in()  / float(RC_SCALE) * g.max_pos_z * blimp.scheduler.get_loop_period_s();
     float pilot_yaw = channel_yaw->get_control_in()  / float(RC_SCALE) * g.max_pos_yaw * blimp.scheduler.get_loop_period_s();
 
     if (g.simple_mode == 0){
         //If simple mode is disabled, input is in body-frame, thus needs to be rotated.
-        blimp.rotate_BF_to_NE(pilot_fwd, pilot_rgt);
+        blimp.rotate_BF_to_NE(pilot.xy());
     }
-    target_pos.x += pilot_fwd;
+    target_pos.x += pilot.x;
-    target_pos.y += pilot_rgt;
+    target_pos.y += pilot.y;
-    target_pos.z += pilot_dwn;
+    target_pos.z += pilot.z;
     target_yaw = wrap_PI(target_yaw + pilot_yaw);
 
     //Pos controller's output becomes target for velocity controller
@@ -38,22 +39,25 @@ void ModeLoiter::run()
 
     Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -g.max_vel_xy, g.max_vel_xy),
                               constrain_float(target_vel_ef.y, -g.max_vel_xy, g.max_vel_xy),
-                              constrain_float(target_vel_ef.z, -g.max_vel_xy, g.max_vel_xy)};
+                              constrain_float(target_vel_ef.z, -g.max_vel_z, g.max_vel_z)};
     float target_vel_yaw_c = constrain_float(target_vel_yaw, -g.max_vel_yaw, g.max_vel_yaw);
 
     Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c, blimp.vel_ned_filtd, {0,0,0});
     float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z);
-    blimp.rotate_NE_to_BF(actuator.x, actuator.y);
+    blimp.rotate_NE_to_BF(actuator);
     float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd);
 
     if(!(blimp.g.dis_mask & (1<<(2-1)))){
-    motors->front_out = actuator.x;
+        motors->front_out = actuator.x;
-    } if(!(blimp.g.dis_mask & (1<<(1-1)))){
+    }
-    motors->right_out = actuator.y;
+    if(!(blimp.g.dis_mask & (1<<(1-1)))){
-    } if(!(blimp.g.dis_mask & (1<<(3-1)))){
+        motors->right_out = actuator.y;
-    motors->down_out = act_down;
+    }
-    } if(!(blimp.g.dis_mask & (1<<(4-1)))){
+    if(!(blimp.g.dis_mask & (1<<(3-1)))){
-    motors->yaw_out  = act_yaw;
+        motors->down_out = act_down;
+    }
+    if(!(blimp.g.dis_mask & (1<<(4-1)))){
+        motors->yaw_out  = act_yaw;
     }
 
     AP::logger().Write_PSC(target_pos*100.0f, blimp.pos_ned*100.0f, target_vel_ef_c*100.0f, blimp.vel_ned_filtd*100.0f, blimp.vel_ned*100.0f, target_yaw*100.0f, yaw_ef*100.0f); //last entries here are just for debugging

Blimp/mode_velocity.cpp

@@ -9,23 +9,26 @@ void ModeVelocity::run()
     Vector3f target_vel;
     target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
     target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
-    blimp.rotate_BF_to_NE(target_vel.x, target_vel.y);
+    blimp.rotate_BF_to_NE(target_vel.xy());
     target_vel.z = channel_down->get_control_in()  / float(RC_SCALE) * g.max_vel_z;
     float target_vel_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_vel_yaw;
 
     Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, {0,0,0});
-    blimp.rotate_NE_to_BF(actuator.x, actuator.y);
+    blimp.rotate_NE_to_BF(actuator);
     float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z);
     float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd);
 
     if(!(blimp.g.dis_mask & (1<<(2-1)))){
-    motors->front_out = actuator.x;
+        motors->front_out = actuator.x;
-    } if(!(blimp.g.dis_mask & (1<<(1-1)))){
+    }
-    motors->right_out = actuator.y;
+    if(!(blimp.g.dis_mask & (1<<(1-1)))){
-    } if(!(blimp.g.dis_mask & (1<<(3-1)))){
+        motors->right_out = actuator.y;
-    motors->down_out = act_down;
+    }
-    } if(!(blimp.g.dis_mask & (1<<(4-1)))){
+    if(!(blimp.g.dis_mask & (1<<(3-1)))){
-    motors->yaw_out = act_yaw;
+        motors->down_out = act_down;
+    }
+    if(!(blimp.g.dis_mask & (1<<(4-1)))){
+        motors->yaw_out = act_yaw;
     }
 
     AP::logger().Write_PSC({0,0,0}, blimp.pos_ned, target_vel*100.0f, blimp.vel_ned_filtd*100.0f, blimp.vel_ned*100.0f, 0, 0);