Blimp: Add Loiter class and RTL mode

Blimp/Blimp.cpp

@@ -217,12 +217,27 @@ void Blimp::read_AHRS(void)
     ahrs.update(true);
 
     IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned));
-    IGNORE_RETURN(ahrs.get_relative_position_NED_home(pos_ned));
+    IGNORE_RETURN(ahrs.get_relative_position_NED_origin(pos_ned));
 
     vel_yaw = ahrs.get_yaw_rate_earth();
     Vector2f vel_xy_filtd = vel_xy_filter.apply({vel_ned.x, vel_ned.y});
     vel_ned_filtd = {vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)};
     vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw);
+
+    AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff",
+                                AP_HAL::micros64(),
+                                vel_ned.x,
+                                vel_ned_filtd.x,
+                                vel_ned.y,
+                                vel_ned_filtd.y,
+                                vel_ned.z,
+                                vel_ned_filtd.z,
+                                vel_yaw,
+                                vel_yaw_filtd,
+                                pos_ned.x,
+                                pos_ned.y,
+                                pos_ned.z,
+                                blimp.ahrs.get_yaw());
 }
 
 // read baro and log control tuning

Blimp/Blimp.h

@@ -59,6 +59,7 @@
 #include "config.h"
 
 #include "Fins.h"
+#include "Loiter.h"
 
 #include "RC_Channel.h"         // RC Channel Library
 
@@ -91,8 +92,10 @@ public:
     friend class ModeLand;
     friend class ModeVelocity;
     friend class ModeLoiter;
+    friend class ModeRTL;
 
     friend class Fins;
+    friend class Loiter;
 
     Blimp(void);
 
@@ -191,6 +194,7 @@ private:
 
     // Motor Output
     Fins *motors;
+    Loiter *loiter;
 
     int32_t _home_bearing;
     uint32_t _home_distance;
@@ -430,6 +434,7 @@ private:
     ModeLand mode_land;
     ModeVelocity mode_velocity;
     ModeLoiter mode_loiter;
+    ModeRTL mode_rtl;
 
     // mode.cpp
     Mode *mode_from_mode_num(const Mode::Number mode);

Blimp/Fins.h

@@ -10,6 +10,7 @@ class Fins
 {
 public:
     friend class Blimp;
+    friend class Loiter;
 
     enum motor_frame_class {
         MOTOR_FRAME_UNDEFINED = 0,

Blimp/Loiter.cpp

@@ -0,0 +1,170 @@
+#include "Blimp.h"
+
+#define MA 0.99
+#define MO (1-MA)
+
+void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled)
+{
+    const float dt = blimp.scheduler.get_last_loop_time_s();
+
+    float scaler_xz_n;
+    float xz_out = fabsf(blimp.motors->front_out) + fabsf(blimp.motors->down_out);
+    if (xz_out > 1) scaler_xz_n = 1 / xz_out;
+    else scaler_xz_n = 1;
+    scaler_xz = scaler_xz*MA + scaler_xz_n*MO;
+
+    float scaler_yyaw_n;
+    float yyaw_out = fabsf(blimp.motors->right_out) + fabsf(blimp.motors->yaw_out);
+    if (yyaw_out > 1) scaler_yyaw_n = 1 / yyaw_out;
+    else scaler_yyaw_n = 1;
+    scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO;
+
+    AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn",
+                                "Qffff",
+                                AP_HAL::micros64(),
+                                scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n);
+
+    float yaw_ef = blimp.ahrs.get_yaw();
+    Vector3f err_xyz = target_pos - blimp.pos_ned;
+    float err_yaw = wrap_PI(target_yaw - yaw_ef);
+
+    Vector4b zero;
+    if((fabsf(err_xyz.x) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) zero.x = true;
+    if((fabsf(err_xyz.y) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) zero.y = true;
+    if((fabsf(err_xyz.z) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) zero.z = true;
+    if((fabsf(err_yaw)   < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) zero.yaw = true;
+
+    //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case."
+    Vector4b limit = zero || axes_disabled;
+
+    Vector3f target_vel_ef;
+    if(!axes_disabled.x && !axes_disabled.y) target_vel_ef = {blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {(float)limit.x, (float)limit.y, (float)limit.z}), 0};
+    if(!axes_disabled.z) target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z);
+
+    float target_vel_yaw = 0;
+    if (!axes_disabled.yaw) {
+        target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt, limit.yaw);
+        blimp.pid_pos_yaw.set_target_rate(target_yaw);
+        blimp.pid_pos_yaw.set_actual_rate(yaw_ef);
+    }
+
+    Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
+                             constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
+                             constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)};
+    float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw);
+
+    Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw};
+    Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw};
+
+    Vector2f actuator;
+    if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y});
+    float act_down = 0;
+    if(!axes_disabled.z) act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z);
+    blimp.rotate_NE_to_BF(actuator);
+    float act_yaw = 0;
+    if(!axes_disabled.yaw) act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw);
+
+    if (!blimp.motors->armed()) {
+        blimp.pid_pos_xy.set_integrator(Vector2f(0,0));
+        blimp.pid_pos_z.set_integrator(0);
+        blimp.pid_pos_yaw.set_integrator(0);
+        blimp.pid_vel_xy.set_integrator(Vector2f(0,0));
+        blimp.pid_vel_z.set_integrator(0);
+        blimp.pid_vel_yaw.set_integrator(0);
+        target_pos = blimp.pos_ned;
+        target_yaw = blimp.ahrs.get_yaw();
+    }
+
+    if (zero.x) {
+        blimp.motors->front_out = 0;
+    } else if (axes_disabled.x);
+    else {
+        blimp.motors->front_out = actuator.x;
+    }
+    if (zero.y) {
+        blimp.motors->right_out = 0;
+    } else if (axes_disabled.y);
+    else {
+        blimp.motors->right_out = actuator.y;
+    }
+    if (zero.z) {
+        blimp.motors->down_out = 0;
+    } else if (axes_disabled.z);
+    else {
+        blimp.motors->down_out = act_down;
+    }
+    if (zero.yaw) {
+        blimp.motors->yaw_out  = 0;
+    } else if (axes_disabled.yaw);
+    else {
+        blimp.motors->yaw_out = act_yaw;
+    }
+
+    AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
+    AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
+    AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
+}
+
+void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled)
+{
+    const float dt = blimp.scheduler.get_last_loop_time_s();
+
+    Vector4b zero;
+    if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) zero.x = true;
+    if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) zero.y = true;
+    if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) zero.z = true;
+    if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) zero.yaw = true;
+    //Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case."
+    Vector4b limit = zero || axes_disabled;
+
+    Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
+                             constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
+                             constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)};
+    float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw);
+
+    Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw};
+    Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw};
+
+    Vector2f actuator;
+    if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y});
+    float act_down = 0;
+    if(!axes_disabled.z) act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z);
+    blimp.rotate_NE_to_BF(actuator);
+    float act_yaw = 0;
+    if(!axes_disabled.yaw) act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw);
+
+    if (!blimp.motors->armed()) {
+        blimp.pid_vel_xy.set_integrator(Vector2f(0,0));
+        blimp.pid_vel_z.set_integrator(0);
+        blimp.pid_vel_yaw.set_integrator(0);
+    }
+
+    if (zero.x) {
+        blimp.motors->front_out = 0;
+    } else if (axes_disabled.x);
+    else {
+        blimp.motors->front_out = actuator.x;
+    }
+    if (zero.y) {
+        blimp.motors->right_out = 0;
+    } else if (axes_disabled.y);
+    else {
+        blimp.motors->right_out = actuator.y;
+    }
+    if (zero.z) {
+        blimp.motors->down_out = 0;
+    } else if (axes_disabled.z);
+    else {
+        blimp.motors->down_out = act_down;
+    }
+    if (zero.yaw) {
+        blimp.motors->yaw_out  = 0;
+    } else if (axes_disabled.yaw);
+    else {
+        blimp.motors->yaw_out = act_yaw;
+    }
+
+    AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
+    AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
+    AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
+}

Blimp/Loiter.h

@@ -0,0 +1,59 @@
+#pragma once
+
+class Vector4b
+{
+public:
+    bool    x;
+    bool    y;
+    bool    z;
+    bool    yaw;
+
+    constexpr Vector4b()
+        : x(0)
+        , y(0)
+        , z(0)
+        , yaw(0) {}
+
+    constexpr Vector4b(const bool x0, const bool y0, const bool z0, const bool yaw0)
+        : x(x0)
+        , y(y0)
+        , z(z0)
+        , yaw(yaw0) {}
+
+    Vector4b operator &&(const Vector4b &v)
+    {
+        Vector4b temp{x && v.x, y && v.y, z && v.z, yaw && v.yaw};
+        return temp;
+    }
+
+    Vector4b operator ||(const Vector4b &v)
+    {
+        Vector4b temp{x || v.x, y || v.y, z || v.z, yaw || v.yaw};
+        return temp;
+    }
+
+};
+
+
+
+class Loiter
+{
+public:
+    friend class Blimp;
+    friend class Fins;
+
+    float scaler_xz;
+    float scaler_yyaw;
+
+    //constructor
+    Loiter(uint16_t loop_rate)
+    {
+        scaler_xz = 1;
+        scaler_yyaw = 1;
+    };
+
+    //Run Loiter controller with target position and yaw in global frame. Expects to be called at loop rate.
+    void run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled);
+    //Run Loiter controller with target velocity and velocity in global frame. Expects to be called at loop rate.
+    void run_vel(Vector3f& target_vel, float& target_vel_yaw, Vector4b axes_disabled);
+};

Blimp/Parameters.cpp

@@ -278,6 +278,14 @@ const AP_Param::Info Blimp::var_info[] = {
     // @User: Standard
     GSCALAR(dis_mask, "DIS_MASK", 0),
 
+    // @Param: PID_DZ
+    // @DisplayName: Deadzone for the position PIDs
+    // @Description: Output 0 thrust signal when blimp is within this distance (in meters) of the target position. Warning: If this param is greater than MAX_POS_XY param then the blimp won't move at all in the XY plane in Loiter mode as it does not allow more than a second's lag. Same for the other axes.
+    // @Units: m
+    // @Range: 0.1 1
+    // @User: Standard
+    GSCALAR(pid_dz, "PID_DZ", 0),
+
     // @Param: RC_SPEED
     // @DisplayName: ESC Update Speed
     // @Description: This is the speed in Hertz that your ESCs will receive updates

Blimp/Parameters.h

@@ -110,6 +110,7 @@ public:
         k_param_max_pos_yaw,
         k_param_simple_mode,
         k_param_dis_mask,
+        k_param_pid_dz,
 
         //
         // 90: misc2
@@ -254,6 +255,7 @@ public:
 
     AP_Int8         simple_mode;
     AP_Int16        dis_mask;
+    AP_Float        pid_dz;
 
     AP_Int8         rtl_alt_type;
 

Blimp/mode.cpp

@@ -16,6 +16,7 @@ Mode::Mode(void) :
     inertial_nav(blimp.inertial_nav),
     ahrs(blimp.ahrs),
     motors(blimp.motors),
+    loiter(blimp.loiter),
     channel_right(blimp.channel_right),
     channel_front(blimp.channel_front),
     channel_down(blimp.channel_down),
@@ -41,6 +42,9 @@ Mode *Blimp::mode_from_mode_num(const Mode::Number mode)
     case Mode::Number::LOITER:
         ret = &mode_loiter;
         break;
+    case Mode::Number::RTL:
+        ret = &mode_rtl;
+        break;
     default:
         break;
     }
@@ -152,18 +156,23 @@ void Mode::update_navigation()
     run_autopilot();
 }
 
-// returns desired angle in centi-degrees
+// returns desired thrust/acceleration
-void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const
+void Mode::get_pilot_input(Vector3f &pilot, float &yaw)
 {
     // throttle failsafe check
     if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) {
-        right_out = 0;
+        pilot.y = 0;
-        front_out = 0;
+        pilot.x = 0;
+        pilot.z = 0;
+        yaw = 0;
         return;
     }
-    // fetch roll and pitch inputs
+    // fetch pilot inputs
-    right_out = channel_right->get_control_in();
+    pilot.y = channel_right->get_control_in() / float(RC_SCALE);
-    front_out = channel_front->get_control_in();
+    pilot.x = channel_front->get_control_in() / float(RC_SCALE);
+    //TODO: need to make this channel_up instead, and then have it .negative. before being sent to pilot.z -> this is "throttle" channel, so higher = up.
+    pilot.z = -channel_up->get_control_in() / float(RC_SCALE);
+    yaw = channel_yaw->get_control_in() / float(RC_SCALE);
 }
 
 bool Mode::is_disarmed_or_landed() const

Blimp/mode.h

@@ -17,6 +17,7 @@ public:
         MANUAL =        1,  // manual control
         VELOCITY =      2,  // velocity mode
         LOITER =        3,  // loiter mode (position hold)
+        RTL =           4,  // rtl
     };
 
     // constructor
@@ -83,7 +84,7 @@ public:
     void update_navigation();
 
     // pilot input processing
-    void get_pilot_desired_accelerations(float &right_out, float &front_out) const;
+    void get_pilot_input(Vector3f &pilot, float &yaw);
 
 protected:
 
@@ -104,6 +105,7 @@ protected:
     AP_InertialNav &inertial_nav;
     AP_AHRS &ahrs;
     Fins *&motors;
+    Loiter *&loiter;
     RC_Channel *&channel_right;
     RC_Channel *&channel_front;
     RC_Channel *&channel_down;
@@ -304,7 +306,6 @@ protected:
 private:
     Vector3f target_pos;
     float target_yaw;
-    float loop_period;
 };
 
 class ModeLand : public Mode
@@ -347,3 +348,43 @@ protected:
 private:
 
 };
+
+class ModeRTL : public Mode
+{
+
+public:
+    // inherit constructor
+    using Mode::Mode;
+
+    virtual bool init(bool ignore_checks) override;
+    virtual void run() override;
+
+    bool requires_GPS() const override
+    {
+        return true;
+    }
+    bool has_manual_throttle() const override
+    {
+        return false;
+    }
+    bool allows_arming(bool from_gcs) const override
+    {
+        return true;
+    };
+    bool is_autopilot() const override
+    {
+        return false;
+        //TODO
+    }
+
+protected:
+
+    const char *name() const override
+    {
+        return "RTL";
+    }
+    const char *name4() const override
+    {
+        return "RTL";
+    }
+};

Blimp/mode_loiter.cpp

@@ -3,13 +3,16 @@
  * Init and run calls for loiter flight mode
  */
 
+//Number of seconds of movement that the target position can be ahead of actual position.
+#define POS_LAG 1
 
- bool ModeLoiter::init(bool ignore_checks){
+bool ModeLoiter::init(bool ignore_checks)
+{
     target_pos = blimp.pos_ned;
     target_yaw = blimp.ahrs.get_yaw();
 
     return true;
- }
+}
 
 //Runs the main loiter controller
 void ModeLoiter::run()
@@ -17,52 +20,22 @@ void ModeLoiter::run()
     const float dt = blimp.scheduler.get_last_loop_time_s();
 
     Vector3f pilot;
-    pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt;
+    float pilot_yaw;
-    pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt;
+    get_pilot_input(pilot, pilot_yaw);
-    pilot.z = channel_down->get_control_in()  / float(RC_SCALE) * g.max_pos_z * dt;
+    pilot.x *= g.max_pos_xy * dt;
-    float pilot_yaw = channel_yaw->get_control_in()  / float(RC_SCALE) * g.max_pos_yaw * dt;
+    pilot.y *= g.max_pos_xy * dt;
+    pilot.z *= g.max_pos_z * dt;
+    pilot_yaw *= g.max_pos_yaw * dt;
 
-    if (g.simple_mode == 0){
+    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.xy());
     }
-    target_pos.x += pilot.x;
-    target_pos.y += pilot.y;
-    target_pos.z += pilot.z;
-    target_yaw = wrap_PI(target_yaw + pilot_yaw);
 
-    //Pos controller's output becomes target for velocity controller
+    if(fabsf(target_pos.x-blimp.pos_ned.x) < (g.max_pos_xy*POS_LAG)) target_pos.x += pilot.x;
-    Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {0,0,0}), 0};
+    if(fabsf(target_pos.y-blimp.pos_ned.y) < (g.max_pos_xy*POS_LAG)) target_pos.y += pilot.y;
-    target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt);
+    if(fabsf(target_pos.z-blimp.pos_ned.z) < (g.max_pos_z*POS_LAG)) target_pos.z += pilot.z;
-    float yaw_ef = blimp.ahrs.get_yaw();
+    if(fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) target_yaw = wrap_PI(target_yaw + pilot_yaw);
-    float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt);
-    blimp.pid_pos_yaw.set_target_rate(target_yaw);
-    blimp.pid_pos_yaw.set_actual_rate(yaw_ef);
 
-    Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -g.max_vel_xy, g.max_vel_xy),
+    blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false});
-                              constrain_float(target_vel_ef.y, -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, dt, {0,0,0});
-    float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z, dt);
-    blimp.rotate_NE_to_BF(actuator);
-    float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd, dt);
-
-    if(!(blimp.g.dis_mask & (1<<(2-1)))){
-        motors->front_out = actuator.x;
-    }
-    if(!(blimp.g.dis_mask & (1<<(1-1)))){
-        motors->right_out = actuator.y;
-    }
-    if(!(blimp.g.dis_mask & (1<<(3-1)))){
-        motors->down_out = act_down;
-    }
-    if(!(blimp.g.dis_mask & (1<<(4-1)))){
-        motors->yaw_out  = act_yaw;
-    }
-
-    AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
-    AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
-    AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
 }

Blimp/mode_manual.cpp

@@ -6,8 +6,11 @@
 // Runs the main manual controller
 void ModeManual::run()
 {
-    motors->right_out = channel_right->get_control_in() / float(RC_SCALE);
+    Vector3f pilot;
-    motors->front_out = channel_front->get_control_in() / float(RC_SCALE);
+    float pilot_yaw;
-    motors->yaw_out = channel_yaw->get_control_in() / float(RC_SCALE);
+    get_pilot_input(pilot, pilot_yaw);
-    motors->down_out = channel_down->get_control_in() / float(RC_SCALE);
+    motors->right_out = pilot.y;
+    motors->front_out = pilot.x;
+    motors->yaw_out = pilot_yaw;
+    motors->down_out = pilot.z;
 }

Blimp/mode_rtl.cpp

@@ -0,0 +1,20 @@
+#include "Blimp.h"
+/*
+ * Init and run calls for rtl flight mode
+ */
+
+//Number of seconds of movement that the target position can be ahead of actual position.
+#define POS_LAG 1
+
+bool ModeRTL::init(bool ignore_checks)
+{
+    return true;
+}
+
+//Runs the main rtl controller
+void ModeRTL::run()
+{
+    Vector3f target_pos = {0,0,0};
+    float target_yaw = 0;
+    blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false});
+}

Blimp/mode_velocity.cpp

@@ -8,34 +8,17 @@
 // Runs the main velocity controller
 void ModeVelocity::run()
 {
-    const float dt = blimp.scheduler.get_last_loop_time_s();
-    
     Vector3f target_vel;
-    target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
+    float target_vel_yaw;
-    target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
+    get_pilot_input(target_vel, target_vel_yaw);
+    target_vel.x *= g.max_vel_xy;
+    target_vel.y *= g.max_vel_xy;
+    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(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;
+    target_vel.z *= g.max_vel_z;
+    target_vel_yaw *= g.max_vel_yaw;
 
-    Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, dt, {0,0,0});
+    blimp.loiter->run_vel(target_vel, target_vel_yaw, Vector4b{false,false,false,false});
-    blimp.rotate_NE_to_BF(actuator);
-    float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z, dt);
-    float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd, dt);
-
-    if(!(blimp.g.dis_mask & (1<<(2-1)))){
-        motors->front_out = actuator.x;
-    }
-    if(!(blimp.g.dis_mask & (1<<(1-1)))){
-        motors->right_out = actuator.y;
-    }
-    if(!(blimp.g.dis_mask & (1<<(3-1)))){
-        motors->down_out = act_down;
-    }
-    if(!(blimp.g.dis_mask & (1<<(4-1)))){
-        motors->yaw_out = act_yaw;
-    }
-
-    AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
-    AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
-    AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, 0.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
 }

Blimp/system.cpp

@@ -46,6 +46,7 @@ void Blimp::init_ardupilot()
 
     // allocate the motors class
     allocate_motors();
+    loiter = new Loiter(blimp.scheduler.get_loop_rate_hz());
 
     // initialise rc channels including setting mode
     rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);