Blimp: initial implementation of blimp vehicle type

Blimp/AP_Arming.cpp

@@ -0,0 +1,488 @@
+#include "Blimp.h"
+
+
+// performs pre-arm checks. expects to be called at 1hz.
+void AP_Arming_Blimp::update(void)
+{
+    // perform pre-arm checks & display failures every 30 seconds
+    static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2;
+    pre_arm_display_counter++;
+    bool display_fail = false;
+    if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) {
+        display_fail = true;
+        pre_arm_display_counter = 0;
+    }
+
+    pre_arm_checks(display_fail);
+}
+
+bool AP_Arming_Blimp::pre_arm_checks(bool display_failure)
+{
+    const bool passed = run_pre_arm_checks(display_failure);
+    set_pre_arm_check(passed);
+    return passed;
+}
+
+// perform pre-arm checks
+//  return true if the checks pass successfully
+bool AP_Arming_Blimp::run_pre_arm_checks(bool display_failure)
+{
+    // exit immediately if already armed
+    if (blimp.motors->armed()) {
+        return true;
+    }
+
+    // check if motor interlock and Emergency Stop aux switches are used
+    // at the same time.  This cannot be allowed.
+    if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) &&
+        rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)) {
+        check_failed(display_failure, "Interlock/E-Stop Conflict");
+        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);
+    }
+
+    return fence_checks(display_failure)
+           & parameter_checks(display_failure)
+           & motor_checks(display_failure)
+           & pilot_throttle_checks(display_failure)
+           & gcs_failsafe_check(display_failure)
+           & alt_checks(display_failure)
+           & AP_Arming::pre_arm_checks(display_failure);
+}
+
+bool AP_Arming_Blimp::barometer_checks(bool display_failure)
+{
+    if (!AP_Arming::barometer_checks(display_failure)) {
+        return false;
+    }
+
+    bool ret = true;
+    // check Baro
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO)) {
+        // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
+        // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
+        // that may differ from the baro height due to baro drift.
+        nav_filter_status filt_status = blimp.inertial_nav.get_filter_status();
+        bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
+        if (using_baro_ref) {
+            if (fabsf(blimp.inertial_nav.get_altitude() - blimp.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
+                check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity");
+                ret = false;
+            }
+        }
+    }
+    return ret;
+}
+
+bool AP_Arming_Blimp::compass_checks(bool display_failure)
+{
+    bool ret = AP_Arming::compass_checks(display_failure);
+
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS)) {
+        // check compass offsets have been set.  AP_Arming only checks
+        // this if learning is off; Blimp *always* checks.
+        char failure_msg[50] = {};
+        if (!AP::compass().configured(failure_msg, ARRAY_SIZE(failure_msg))) {
+            check_failed(ARMING_CHECK_COMPASS, display_failure, "%s", failure_msg);
+            ret = false;
+        }
+    }
+
+    return ret;
+}
+
+bool AP_Arming_Blimp::ins_checks(bool display_failure)
+{
+    bool ret = AP_Arming::ins_checks(display_failure);
+
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) {
+
+        // get ekf attitude (if bad, it's usually the gyro biases)
+        if (!pre_arm_ekf_attitude_check()) {
+            check_failed(ARMING_CHECK_INS, display_failure, "EKF attitude is bad");
+            ret = false;
+        }
+    }
+
+    return ret;
+}
+
+bool AP_Arming_Blimp::board_voltage_checks(bool display_failure)
+{
+    if (!AP_Arming::board_voltage_checks(display_failure)) {
+        return false;
+    }
+
+    // check battery voltage
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) {
+        if (blimp.battery.has_failsafed()) {
+            check_failed(ARMING_CHECK_VOLTAGE, display_failure, "Battery failsafe");
+            return false;
+        }
+
+        // call parent battery checks
+        if (!AP_Arming::battery_checks(display_failure)) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool AP_Arming_Blimp::parameter_checks(bool display_failure)
+{
+    // check various parameter values
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) {
+
+        // failsafe parameter checks
+        if (blimp.g.failsafe_throttle) {
+            // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
+            if (blimp.channel_down->get_radio_min() <= blimp.g.failsafe_throttle_value+10 || blimp.g.failsafe_throttle_value < 910) {
+                check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE");
+                return false;
+            }
+        }
+        if (blimp.g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
+            // FS_GCS_ENABLE == 2 has been removed
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "FS_GCS_ENABLE=2 removed, see FS_OPTIONS");
+        }
+
+        // lean angle parameter check
+        if (blimp.aparm.angle_max < 1000 || blimp.aparm.angle_max > 8000) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ANGLE_MAX");
+            return false;
+        }
+
+        // pilot-speed-up parameter check
+        if (blimp.g.pilot_speed_up <= 0) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check PILOT_SPEED_UP");
+            return false;
+        }
+
+        // checks MOT_PWM_MIN/MAX for acceptable values
+        // if (!blimp.motors->check_mot_pwm_params()) {
+        //     check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check MOT_PWM_MIN/MAX");
+        //     return false;
+        // }
+
+        // ensure controllers are OK with us arming:
+        // char failure_msg[50];
+        // if (!blimp.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
+        //     check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
+        //     return false;
+        // }
+        // if (!blimp.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
+        //     check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
+        //     return false;
+        //}
+    }
+
+    return true;
+}
+
+// check motor setup was successful
+bool AP_Arming_Blimp::motor_checks(bool display_failure)
+{
+    // check motors initialised  correctly
+    if (!blimp.motors->initialised_ok()) {
+        check_failed(display_failure, "Check firmware or FRAME_CLASS");
+        return false;
+    }
+
+    // further checks enabled with parameters
+    if (!check_enabled(ARMING_CHECK_PARAMETERS)) {
+        return true;
+    }
+
+    return true;
+}
+
+bool AP_Arming_Blimp::pilot_throttle_checks(bool display_failure)
+{
+    // check throttle is above failsafe throttle
+    // this is near the bottom to allow other failures to be displayed before checking pilot throttle
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) {
+        if (blimp.g.failsafe_throttle != FS_THR_DISABLED && blimp.channel_down->get_radio_in() < blimp.g.failsafe_throttle_value) {
+            const char *failmsg = "Throttle below Failsafe";
+            check_failed(ARMING_CHECK_RC, display_failure, "%s", failmsg);
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool AP_Arming_Blimp::rc_calibration_checks(bool display_failure)
+{
+    return true;
+}
+
+// performs pre_arm gps related checks and returns true if passed
+bool AP_Arming_Blimp::gps_checks(bool display_failure)
+{
+    // run mandatory gps checks first
+    if (!mandatory_gps_checks(display_failure)) {
+        AP_Notify::flags.pre_arm_gps_check = false;
+        return false;
+    }
+
+    // check if flight mode requires GPS
+    bool mode_requires_gps = blimp.flightmode->requires_GPS();
+
+
+    // return true if GPS is not required
+    if (!mode_requires_gps) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
+
+    // return true immediately if gps check is disabled
+    if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS)) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
+
+    // warn about hdop separately - to prevent user confusion with no gps lock
+    if (blimp.gps.get_hdop() > blimp.g.gps_hdop_good) {
+        check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP");
+        AP_Notify::flags.pre_arm_gps_check = false;
+        return false;
+    }
+
+    // call parent gps checks
+    if (!AP_Arming::gps_checks(display_failure)) {
+        AP_Notify::flags.pre_arm_gps_check = false;
+        return false;
+    }
+
+    // if we got here all must be ok
+    AP_Notify::flags.pre_arm_gps_check = true;
+    return true;
+}
+
+// check ekf attitude is acceptable
+bool AP_Arming_Blimp::pre_arm_ekf_attitude_check()
+{
+    // get ekf filter status
+    nav_filter_status filt_status = blimp.inertial_nav.get_filter_status();
+
+    return filt_status.flags.attitude;
+}
+
+// performs mandatory gps checks.  returns true if passed
+bool AP_Arming_Blimp::mandatory_gps_checks(bool display_failure)
+{
+    // always check if inertial nav has started and is ready
+    const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
+    char failure_msg[50] = {};
+    if (!ahrs.pre_arm_check(false, failure_msg, sizeof(failure_msg))) {
+        check_failed(display_failure, "AHRS: %s", failure_msg);
+        return false;
+    }
+
+    // check if flight mode requires GPS
+    bool mode_requires_gps = blimp.flightmode->requires_GPS();
+
+    if (mode_requires_gps) {
+        if (!blimp.position_ok()) {
+            // vehicle level position estimate checks
+            check_failed(display_failure, "Need Position Estimate");
+            return false;
+        }
+    } else  {
+        // return true if GPS is not required
+        return true;
+    }
+
+    // if we got here all must be ok
+    return true;
+}
+
+// Check GCS failsafe
+bool AP_Arming_Blimp::gcs_failsafe_check(bool display_failure)
+{
+    if (blimp.failsafe.gcs) {
+        check_failed(display_failure, "GCS failsafe on");
+        return false;
+    }
+    return true;
+}
+
+// performs altitude checks.  returns true if passed
+bool AP_Arming_Blimp::alt_checks(bool display_failure)
+{
+    // always EKF altitude estimate
+    if (!blimp.flightmode->has_manual_throttle() && !blimp.ekf_alt_ok()) {
+        check_failed(display_failure, "Need Alt Estimate");
+        return false;
+    }
+
+    return true;
+}
+
+// arm_checks - perform final checks before arming
+//  always called just before arming.  Return true if ok to arm
+//  has side-effect that logging is started
+bool AP_Arming_Blimp::arm_checks(AP_Arming::Method method)
+{
+    return AP_Arming::arm_checks(method);
+}
+
+// mandatory checks that will be run if ARMING_CHECK is zero or arming forced
+bool AP_Arming_Blimp::mandatory_checks(bool display_failure)
+{
+    // call mandatory gps checks and update notify status because regular gps checks will not run
+    bool result = mandatory_gps_checks(display_failure);
+    AP_Notify::flags.pre_arm_gps_check = result;
+
+    // call mandatory alt check
+    if (!alt_checks(display_failure)) {
+        result = false;
+    }
+
+    return result;
+}
+
+void AP_Arming_Blimp::set_pre_arm_check(bool b)
+{
+    blimp.ap.pre_arm_check = b;
+    AP_Notify::flags.pre_arm_check = b;
+}
+
+bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_checks)
+{
+    static bool in_arm_motors = false;
+
+    // exit immediately if already in this function
+    if (in_arm_motors) {
+        return false;
+    }
+    in_arm_motors = true;
+
+    // return true if already armed
+    if (blimp.motors->armed()) {
+        in_arm_motors = false;
+        return true;
+    }
+
+    if (!AP_Arming::arm(method, do_arming_checks)) {
+        AP_Notify::events.arming_failed = true;
+        in_arm_motors = false;
+        return false;
+    }
+
+    // let logger know that we're armed (it may open logs e.g.)
+    AP::logger().set_vehicle_armed(true);
+
+    // notify that arming will occur (we do this early to give plenty of warning)
+    AP_Notify::flags.armed = true;
+    // call notify update a few times to ensure the message gets out
+    for (uint8_t i=0; i<=10; i++) {
+        AP::notify().update();
+    }
+
+    gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); //MIR kept in - usually only in SITL
+
+    AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
+
+    blimp.initial_armed_bearing = ahrs.yaw_sensor;
+
+    if (!ahrs.home_is_set()) {
+        // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
+        ahrs.resetHeightDatum();
+        AP::logger().Write_Event(LogEvent::EKF_ALT_RESET);
+
+        // we have reset height, so arming height is zero
+        blimp.arming_altitude_m = 0;
+    } else if (!ahrs.home_is_locked()) {
+        // Reset home position if it has already been set before (but not locked)
+        if (!blimp.set_home_to_current_location(false)) {
+            // ignore failure
+        }
+
+        // remember the height when we armed
+        blimp.arming_altitude_m = blimp.inertial_nav.get_altitude() * 0.01;
+    }
+
+    // enable gps velocity based centrefugal force compensation
+    ahrs.set_correct_centrifugal(true);
+    hal.util->set_soft_armed(true);
+
+    // finally actually arm the motors
+    blimp.motors->armed(true);
+
+    // log flight mode in case it was changed while vehicle was disarmed
+    AP::logger().Write_Mode((uint8_t)blimp.control_mode, blimp.control_mode_reason);
+
+    // perf monitor ignores delay due to arming
+    AP::scheduler().perf_info.ignore_this_loop();
+
+    // flag exiting this function
+    in_arm_motors = false;
+
+    // Log time stamp of arming event
+    blimp.arm_time_ms = millis();
+
+    // Start the arming delay
+    blimp.ap.in_arming_delay = true;
+
+    // assumed armed without a arming, switch. Overridden in switches.cpp
+    blimp.ap.armed_with_switch = false;
+
+    // return success
+    return true;
+}
+
+// arming.disarm - disarm motors
+bool AP_Arming_Blimp::disarm(const AP_Arming::Method method, bool do_disarm_checks)
+{
+    // return immediately if we are already disarmed
+    if (!blimp.motors->armed()) {
+        return true;
+    }
+
+    if (!AP_Arming::disarm(method, do_disarm_checks)) {
+        return false;
+    }
+
+    gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors"); //MIR keeping in - usually only in SITL
+
+
+    AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
+
+    // save compass offsets learned by the EKF if enabled
+    Compass &compass = AP::compass();
+    if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
+        for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+            Vector3f magOffsets;
+            if (ahrs.getMagOffsets(i, magOffsets)) {
+                compass.set_and_save_offsets(i, magOffsets);
+            }
+        }
+    }
+
+    // send disarm command to motors
+    blimp.motors->armed(false);
+
+    AP::logger().set_vehicle_armed(false);
+
+    // disable gps velocity based centrefugal force compensation
+    ahrs.set_correct_centrifugal(false);
+    hal.util->set_soft_armed(false);
+
+    blimp.ap.in_arming_delay = false;
+
+    return true;
+}

Blimp/AP_Arming.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include <AP_Arming/AP_Arming.h>
+
+class AP_Arming_Blimp : public AP_Arming
+{
+public:
+    friend class Blimp;
+    friend class ToyMode;
+
+    AP_Arming_Blimp() : AP_Arming()
+    {
+        // default REQUIRE parameter to 1 (Blimp does not have an
+        // actual ARMING_REQUIRE parameter)
+        require.set_default((uint8_t)Required::YES_MIN_PWM);
+    }
+
+    /* Do not allow copies */
+    AP_Arming_Blimp(const AP_Arming_Blimp &other) = delete;
+    AP_Arming_Blimp &operator=(const AP_Arming_Blimp&) = delete;
+
+    void update(void);
+
+    bool rc_calibration_checks(bool display_failure) override;
+
+    bool disarm(AP_Arming::Method method, bool do_disarm_checks=true) override;
+    bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
+
+protected:
+
+    bool pre_arm_checks(bool display_failure) override;
+    bool pre_arm_ekf_attitude_check();
+    // bool proximity_checks(bool display_failure) const override;
+    bool arm_checks(AP_Arming::Method method) override;
+
+    // mandatory checks that cannot be bypassed.  This function will only be called if ARMING_CHECK is zero or arming forced
+    bool mandatory_checks(bool display_failure) override;
+
+    // NOTE! the following check functions *DO* call into AP_Arming:
+    bool ins_checks(bool display_failure) override;
+    bool compass_checks(bool display_failure) override;
+    bool gps_checks(bool display_failure) override;
+    bool barometer_checks(bool display_failure) override;
+    bool board_voltage_checks(bool display_failure) override;
+
+    // NOTE! the following check functions *DO NOT* call into AP_Arming!
+    bool parameter_checks(bool display_failure);
+    bool motor_checks(bool display_failure);
+    bool pilot_throttle_checks(bool display_failure);
+    bool oa_checks(bool display_failure);
+    bool mandatory_gps_checks(bool display_failure);
+    bool gcs_failsafe_check(bool display_failure);
+    bool alt_checks(bool display_failure);
+
+    void set_pre_arm_check(bool b);
+
+private:
+
+    // actually contains the pre-arm checks.  This is wrapped so that
+    // we can store away success/failure of the checks.
+    bool run_pre_arm_checks(bool display_failure);
+
+};

Blimp/AP_State.cpp

@@ -0,0 +1,51 @@
+#include "Blimp.h"
+
+// ---------------------------------------------
+void Blimp::set_auto_armed(bool b)
+{
+    // if no change, exit immediately
+    if ( ap.auto_armed == b ) {
+        return;
+    }
+
+    ap.auto_armed = b;
+    if (b) {
+        AP::logger().Write_Event(LogEvent::AUTO_ARMED);
+    }
+}
+
+// ---------------------------------------------
+void Blimp::set_failsafe_radio(bool b)
+{
+    // only act on changes
+    // -------------------
+    if (failsafe.radio != b) {
+
+        // store the value so we don't trip the gate twice
+        // -----------------------------------------------
+        failsafe.radio = b;
+
+        if (failsafe.radio == false) {
+            // We've regained radio contact
+            // ----------------------------
+            failsafe_radio_off_event();
+        } else {
+            // We've lost radio contact
+            // ------------------------
+            failsafe_radio_on_event();
+        }
+
+        // update AP_Notify
+        AP_Notify::flags.failsafe_radio = b;
+    }
+}
+
+
+// ---------------------------------------------
+void Blimp::set_failsafe_gcs(bool b)
+{
+    failsafe.gcs = b;
+
+    // update AP_Notify
+    AP_Notify::flags.failsafe_gcs = b;
+}

Blimp/Blimp.cpp

@@ -0,0 +1,391 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "Blimp.h"
+
+#define FORCE_VERSION_H_INCLUDE
+#include "version.h"
+#undef FORCE_VERSION_H_INCLUDE
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Blimp, &blimp, func, rate_hz, max_time_micros)
+
+/*
+  scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
+  should be listed here, along with how often they should be called (in hz)
+  and the maximum time they are expected to take (in microseconds)
+ */
+const AP_Scheduler::Task Blimp::scheduler_tasks[] = {
+    SCHED_TASK(rc_loop,              100,    130),
+    SCHED_TASK(throttle_loop,         50,     75),
+    SCHED_TASK_CLASS(AP_GPS, &blimp.gps, update, 50, 200),
+    SCHED_TASK(update_batt_compass,   10,    120),
+    SCHED_TASK_CLASS(RC_Channels,          (RC_Channels*)&blimp.g2.rc_channels,      read_aux_all,    10,     50),
+    SCHED_TASK(arm_motors_check,      10,     50),
+    // SCHED_TASK(auto_disarm_check,     10,     50),
+    // SCHED_TASK(auto_trim,             10,     75),
+    SCHED_TASK(update_altitude,       10,    100),
+    // SCHED_TASK(run_nav_updates,       50,    100),
+    // SCHED_TASK(update_throttle_hover,100,     90),
+    SCHED_TASK(three_hz_loop,          3,     75),
+    SCHED_TASK_CLASS(AP_ServoRelayEvents,  &blimp.ServoRelayEvents,      update_events, 50,     75),
+    SCHED_TASK_CLASS(AP_Baro,              &blimp.barometer,           accumulate,      50,  90),
+#if LOGGING_ENABLED == ENABLED
+    SCHED_TASK(fourhundred_hz_logging,400,    50),
+#endif
+    SCHED_TASK_CLASS(AP_Notify,            &blimp.notify,              update,          50,  90),
+    SCHED_TASK(one_hz_loop,            1,    100),
+    SCHED_TASK(ekf_check,             10,     75),
+    SCHED_TASK(check_vibration,       10,     50),
+    // SCHED_TASK(gpsglitch_check,       10,     50),
+    // SCHED_TASK(landinggear_update,    10,     75),
+    // SCHED_TASK(standby_update,        100,    75),
+    // SCHED_TASK(lost_vehicle_check,    10,     50),
+    SCHED_TASK_CLASS(GCS,                  (GCS*)&blimp._gcs,          update_receive, 400, 180),
+    SCHED_TASK_CLASS(GCS,                  (GCS*)&blimp._gcs,          update_send,    400, 550),
+#if LOGGING_ENABLED == ENABLED
+    SCHED_TASK(ten_hz_logging_loop,   10,    350),
+    SCHED_TASK(twentyfive_hz_logging, 25,    110),
+    SCHED_TASK_CLASS(AP_Logger,      &blimp.logger,           periodic_tasks, 400, 300),
+#endif
+    SCHED_TASK_CLASS(AP_InertialSensor,    &blimp.ins,                 periodic,       400,  50),
+
+    SCHED_TASK_CLASS(AP_Scheduler,         &blimp.scheduler,           update_logging, 0.1,  75),
+    SCHED_TASK(compass_cal_update,   100,    100),
+    SCHED_TASK(accel_cal_update,      10,    100),
+    // SCHED_TASK_CLASS(AP_TempCalibration,   &blimp.g2.temp_calibration, update,          10, 100),
+
+#if STATS_ENABLED == ENABLED
+    SCHED_TASK_CLASS(AP_Stats,             &blimp.g2.stats,            update,           1, 100),
+#endif
+};
+
+void Blimp::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
+                                uint8_t &task_count,
+                                uint32_t &log_bit)
+{
+    tasks = &scheduler_tasks[0];
+    task_count = ARRAY_SIZE(scheduler_tasks);
+    log_bit = MASK_LOG_PM;
+}
+
+constexpr int8_t Blimp::_failsafe_priorities[4];
+
+// Main loop - 50hz
+void Blimp::fast_loop()
+{
+    // update INS immediately to get current gyro data populated
+    ins.update();
+
+    // run low level rate controllers that only require IMU data
+    // attitude_control->rate_controller_run();
+
+    // send outputs to the motors library immediately
+    motors_output();
+
+    // run EKF state estimator (expensive)
+    // --------------------
+    read_AHRS();
+
+    // Inertial Nav
+    // --------------------
+    read_inertia();
+
+    // check if ekf has reset target heading or position
+    check_ekf_reset();
+
+    // run the attitude controllers
+    update_flight_mode();
+
+    // update home from EKF if necessary
+    update_home_from_EKF();
+
+    // check if we've landed or crashed
+    // Skip for now since Blimp won't land
+    // update_land_and_crash_detectors();
+
+    // log sensor health
+    if (should_log(MASK_LOG_ANY)) {
+        Log_Sensor_Health();
+    }
+
+    AP_Vehicle::fast_loop(); //just does gyro fft
+}
+
+// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
+//copied in from Copter's Attitude.cpp
+float Blimp::get_non_takeoff_throttle()
+{
+    // return MAX(0,motors->get_throttle_hover()/2.0f);
+    return 0.0f; //MIR no idle throttle.
+}
+
+// start takeoff to given altitude (for use by scripting)
+// bool Blimp::start_takeoff(float alt)
+// {
+//     // exit if vehicle is not in Guided mode or Auto-Guided mode
+//     if (!flightmode->in_guided_mode()) {
+//         return false;
+//     }
+
+//     if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
+//         blimp.set_auto_armed(true);
+//         return true;
+//     }
+//     return false;
+// }
+
+// set target location (for use by scripting)
+// bool Blimp::set_target_location(const Location& target_loc)
+// {
+//     // exit if vehicle is not in Guided mode or Auto-Guided mode
+//     if (!flightmode->in_guided_mode()) {
+//         return false;
+//     }
+
+//     return mode_guided.set_destination(target_loc);
+// }
+
+// bool Blimp::set_target_velocity_NED(const Vector3f& vel_ned)
+// {
+//     // exit if vehicle is not in Guided mode or Auto-Guided mode
+//     if (!flightmode->in_guided_mode()) {
+//         return false;
+//     }
+
+//     // convert vector to neu in cm
+//     const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
+//     mode_guided.set_velocity(vel_neu_cms);
+//     return true;
+// }
+
+// bool Blimp::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
+// {
+//     // exit if vehicle is not in Guided mode or Auto-Guided mode
+//     if (!flightmode->in_guided_mode()) {
+//         return false;
+//     }
+
+//     Quaternion q;
+//     q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
+
+//     mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false);
+//     return true;
+// }
+
+
+// rc_loops - reads user input from transmitter/receiver
+// called at 100hz
+void Blimp::rc_loop()
+{
+    // Read radio and 3-position switch on radio
+    // -----------------------------------------
+    read_radio();
+    rc().read_mode_switch();
+}
+
+// throttle_loop - should be run at 50 hz
+// ---------------------------
+void Blimp::throttle_loop()
+{
+    // update throttle_low_comp value (controls priority of throttle vs attitude control)
+    // update_throttle_mix();
+
+    // check auto_armed status
+    update_auto_armed();
+}
+
+// update_batt_compass - read battery and compass
+// should be called at 10hz
+void Blimp::update_batt_compass(void)
+{
+    // read battery before compass because it may be used for motor interference compensation
+    battery.read();
+
+    if (AP::compass().enabled()) {
+        // update compass with throttle value - used for compassmot
+        // compass.set_throttle(motors->get_throttle());
+        compass.set_voltage(battery.voltage());
+        compass.read();
+    }
+}
+
+// Full rate logging of attitude, rate and pid loops
+// should be run at 400hz
+void Blimp::fourhundred_hz_logging()
+{
+    if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) {
+        Log_Write_Attitude();
+    }
+}
+
+// ten_hz_logging_loop
+// should be run at 10hz
+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()) {
+        Log_Write_Attitude();
+    }
+    // log EKF attitude data
+    if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
+        Log_Write_EKF_POS();
+    }
+    if (should_log(MASK_LOG_MOTBATT)) {
+        Log_Write_MotBatt();
+    }
+    if (should_log(MASK_LOG_RCIN)) {
+        logger.Write_RCIN();
+        if (rssi.enabled()) {
+            logger.Write_RSSI();
+        }
+    }
+    if (should_log(MASK_LOG_RCOUT)) {
+        logger.Write_RCOUT();
+    }
+    if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
+        logger.Write_Vibration();
+    }
+}
+
+
+// twentyfive_hz_logging - should be run at 25hz
+void Blimp::twentyfive_hz_logging()
+{
+#if HIL_MODE != HIL_MODE_DISABLED
+    // HIL for a blimp needs very fast update of the servo values
+    gcs().send_message(MSG_SERVO_OUTPUT_RAW);
+#endif
+
+#if HIL_MODE == HIL_MODE_DISABLED
+    if (should_log(MASK_LOG_ATTITUDE_FAST)) {
+        Log_Write_EKF_POS();
+    }
+
+    if (should_log(MASK_LOG_IMU)) {
+        logger.Write_IMU();
+    }
+#endif
+
+}
+
+// three_hz_loop - 3.3hz loop
+void Blimp::three_hz_loop()
+{
+    // check if we've lost contact with the ground station
+    failsafe_gcs_check();
+
+    // check if we've lost terrain data
+    // failsafe_terrain_check();
+}
+
+// one_hz_loop - runs at 1Hz
+void Blimp::one_hz_loop()
+{
+    if (should_log(MASK_LOG_ANY)) {
+        Log_Write_Data(LogDataID::AP_STATE, ap.value);
+    }
+
+    arming.update();
+
+    if (!motors->armed()) {
+        // make it possible to change ahrs orientation at runtime during initial config
+        ahrs.update_orientation();
+
+        // update_using_interlock();
+    }
+
+    // update assigned functions and enable auxiliary servos
+    SRV_Channels::enable_aux_servos();
+
+    AP_Notify::flags.flying = !ap.land_complete;
+}
+
+void Blimp::read_AHRS(void)
+{
+    // Perform IMU calculations and get attitude info
+    //-----------------------------------------------
+#if HIL_MODE != HIL_MODE_DISABLED
+    // update hil before ahrs update
+    gcs().update();
+#endif
+
+    // we tell AHRS to skip INS update as we have already done it in fast_loop()
+    ahrs.update(true);
+}
+
+// read baro and log control tuning
+void Blimp::update_altitude()
+{
+    // read in baro altitude
+    read_barometer();
+
+    if (should_log(MASK_LOG_CTUN)) {
+        Log_Write_Control_Tuning();
+#if HAL_GYROFFT_ENABLED
+        gyro_fft.write_log_messages();
+#else
+        write_notch_log_messages();
+#endif
+    }
+}
+
+// vehicle specific waypoint info helpers
+bool Blimp::get_wp_distance_m(float &distance) const
+{
+    // see GCS_MAVLINK_Blimp::send_nav_controller_output()
+    distance = flightmode->wp_distance() * 0.01;
+    return true;
+}
+
+// vehicle specific waypoint info helpers
+bool Blimp::get_wp_bearing_deg(float &bearing) const
+{
+    // see GCS_MAVLINK_Blimp::send_nav_controller_output()
+    bearing = flightmode->wp_bearing() * 0.01;
+    return true;
+}
+
+// vehicle specific waypoint info helpers
+bool Blimp::get_wp_crosstrack_error_m(float &xtrack_error) const
+{
+    // see GCS_MAVLINK_Blimp::send_nav_controller_output()
+    xtrack_error = flightmode->crosstrack_error() * 0.01;
+    return true;
+}
+
+/*
+  constructor for main Blimp class
+ */
+Blimp::Blimp(void)
+    : logger(g.log_bitmask),
+      flight_modes(&g.flight_mode1),
+      control_mode(Mode::Number::MANUAL),
+      land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
+      rc_throttle_control_in_filter(1.0f),
+      inertial_nav(ahrs),
+      param_loader(var_info),
+      flightmode(&mode_manual)
+{
+    // init sensor error logging flags
+    sensor_health.baro = true;
+    sensor_health.compass = true;
+}
+
+Blimp blimp;
+AP_Vehicle& vehicle = blimp;
+
+AP_HAL_MAIN_CALLBACKS(&blimp);

Blimp/Blimp.h

@@ -0,0 +1,544 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+/*
+  This is the main Blimp class
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+// Header includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <stdio.h>
+#include <stdarg.h>
+
+#include <AP_HAL/AP_HAL.h>
+
+// Common dependencies
+#include <AP_Common/AP_Common.h>
+#include <AP_Common/Location.h>
+#include <AP_Param/AP_Param.h>
+#include <StorageManager/StorageManager.h>
+
+// Application dependencies
+#include <GCS_MAVLink/GCS.h>
+#include <AP_Logger/AP_Logger.h>          // ArduPilot Mega Flash Memory Library
+#include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+// #include <AP_AccelCal/AP_AccelCal.h>                // interface and maths for accelerometer calibration
+// #include <AP_InertialSensor/AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
+#include <AP_AHRS/AP_AHRS.h>
+// #include <AP_Mission/AP_Mission.h>     // Mission command library
+// #include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
+// #include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
+// #include <AC_AttitudeControl/AC_PosControl.h>      // Position control library
+// #include <AP_Motors/AP_Motors.h>          // AP Motors library
+#include <AP_Stats/AP_Stats.h>     // statistics library
+#include <Filter/Filter.h>             // Filter library
+#include <AP_Airspeed/AP_Airspeed.h>        // needed for AHRS build
+#include <AP_Vehicle/AP_Vehicle.h>         // needed for AHRS build
+#include <AP_InertialNav/AP_InertialNav.h>     // ArduPilot Mega inertial navigation library
+// #include <AC_WPNav/AC_WPNav.h>           // Blimp waypoint navigation library
+// #include <AC_WPNav/AC_Loiter.h>
+// #include <AC_WPNav/AC_Circle.h>          // circle navigation library
+// #include <AP_Declination/AP_Declination.h>     // ArduPilot Mega Declination Helper Library
+#include <AP_RCMapper/AP_RCMapper.h>        // RC input mapping library
+#include <AP_BattMonitor/AP_BattMonitor.h>     // Battery monitor library
+// #include <AP_LandingGear/AP_LandingGear.h>     // Landing Gear library
+// #include <AC_InputManager/AC_InputManager.h>        // Pilot input handling library
+// #include <AC_InputManager/AC_InputManager_Heli.h>   // Heli specific pilot input handling library
+#include <AP_Arming/AP_Arming.h>
+// #include <AP_SmartRTL/AP_SmartRTL.h>
+// #include <AP_TempCalibration/AP_TempCalibration.h>
+// #include <AC_AutoTune/AC_AutoTune.h>
+// #include <AP_Parachute/AP_Parachute.h>
+// #include <AC_Sprayer/AC_Sprayer.h>
+// #include <AP_ADSB/AP_ADSB.h>
+#include <AP_Scripting/AP_Scripting.h>
+
+// Configuration
+#include "defines.h"
+#include "config.h"
+
+#include "Fins.h"
+
+// #define MOTOR_CLASS Fins
+
+#include "RC_Channel.h"         // RC Channel Library
+
+#include "GCS_Mavlink.h"
+#include "GCS_Blimp.h"
+// #include "AP_Rally.h"           // Rally point library
+#include "AP_Arming.h"
+
+
+#include <AP_Mount/AP_Mount.h>
+
+// Local modules
+
+#include "Parameters.h"
+
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <SITL/SITL.h>
+#endif
+
+#include "mode.h"
+
+class Blimp : public AP_Vehicle
+{
+public:
+    friend class GCS_MAVLINK_Blimp;
+    friend class GCS_Blimp;
+    friend class Parameters;
+    friend class ParametersG2;
+
+    friend class AP_Arming_Blimp;
+    friend class RC_Channel_Blimp;
+    friend class RC_Channels_Blimp;
+
+    friend class Mode;
+    friend class ModeManual;
+    friend class ModeLand;
+
+    friend class Fins;
+
+    Blimp(void);
+
+private:
+
+    // key aircraft parameters passed to multiple libraries
+    AP_Vehicle::MultiCopter aparm;
+
+    // Global parameters are all contained within the 'g' class.
+    Parameters g;
+    ParametersG2 g2;
+
+    // used to detect MAVLink acks from GCS to stop compassmot
+    uint8_t command_ack_counter;
+
+    // primary input control channels
+    RC_Channel *channel_right;
+    RC_Channel *channel_front;
+    RC_Channel *channel_down;
+    RC_Channel *channel_yaw;
+
+    AP_Logger logger;
+
+    // flight modes convenience array
+    AP_Int8 *flight_modes;
+    const uint8_t num_flight_modes = 6;
+
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    SITL::SITL sitl;
+#endif
+
+    // Arming/Disarming management class
+    AP_Arming_Blimp arming;
+
+    // system time in milliseconds of last recorded yaw reset from ekf
+    uint32_t ekfYawReset_ms;
+    int8_t ekf_primary_core;
+
+    // vibration check
+    struct {
+        bool high_vibes;    // true while high vibration are detected
+        uint32_t start_ms;  // system time high vibration were last detected
+        uint32_t clear_ms;  // system time high vibrations stopped
+    } vibration_check;
+
+    // GCS selection
+    GCS_Blimp _gcs; // avoid using this; use gcs()
+    GCS_Blimp &gcs()
+    {
+        return _gcs;
+    }
+
+    // Documentation of Globals:
+    typedef union {
+        struct {
+            uint8_t unused1                 : 1; // 0
+            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; // 4       // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
+            uint8_t auto_armed              : 1; // 5       // stops auto missions from beginning until throttle is raised
+            uint8_t logging_started         : 1; // 6       // true if logging has started
+            uint8_t land_complete           : 1; // 7       // true if we have detected a landing
+            uint8_t new_radio_frame         : 1; // 8       // Set true if we have new PWM data to act on from the Radio
+            uint8_t usb_connected_unused    : 1; // 9       // UNUSED
+            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 compass_mot             : 1; // 11      // true if we are currently performing compassmot calibration
+            uint8_t motor_test              : 1; // 12      // true if we are currently performing the motors test
+            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 land_complete_maybe     : 1; // 14      // true if we may have landed (less strict version of land_complete)
+            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 system_time_set_unused  : 1; // 16      // true if the system time has been set from the GPS
+            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 armed_with_switch       : 1; // 27      // we armed using a arming switch
+        };
+        uint32_t value;
+    } ap_t;
+
+    ap_t ap; //MIR Set of general variables
+
+    static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t");
+
+    // This is the state of the flight control system
+    // There are multiple states defined such as STABILIZE, ACRO,
+    Mode::Number control_mode;
+    ModeReason control_mode_reason = ModeReason::UNKNOWN;
+    Mode::Number prev_control_mode;
+
+    RCMapper rcmap;
+
+    // intertial nav alt when we armed
+    float arming_altitude_m;
+
+    // Failsafe
+    struct {
+        uint32_t last_heartbeat_ms;      // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
+
+        int8_t radio_counter;            // number of iterations with throttle below throttle_fs_value
+
+        uint8_t radio               : 1; // A status flag for the radio failsafe
+        uint8_t gcs                 : 1; // A status flag for the ground station failsafe
+        uint8_t ekf                 : 1; // true if ekf failsafe has occurred
+    } failsafe;
+
+    bool any_failsafe_triggered() const
+    {
+        return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf;
+    }
+
+    // sensor health for logging
+    struct {
+        uint8_t baro        : 1;    // true if baro is healthy
+        uint8_t compass     : 1;    // true if compass is healthy
+        uint8_t primary_gps : 2;    // primary gps index
+    } sensor_health;
+
+    // Motor Output
+    Fins *motors;
+
+    int32_t _home_bearing;
+    uint32_t _home_distance;
+
+    // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable
+    int32_t initial_armed_bearing;
+
+    // Battery Sensors
+    AP_BattMonitor battery{MASK_LOG_CURRENT,
+                       FUNCTOR_BIND_MEMBER(&Blimp::handle_battery_failsafe, void, const char*, const int8_t),
+                       _failsafe_priorities};
+
+    // Altitude
+    int32_t baro_alt;            // barometer altitude in cm above home
+    LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
+
+    // filtered pilot's throttle input used to cancel landing if throttle held high
+    LowPassFilterFloat rc_throttle_control_in_filter;
+
+    // 3D Location vectors
+    // Current location of the vehicle (altitude is relative to home)
+    Location current_loc;
+
+    // Inertial Navigation
+    AP_InertialNav_NavEKF inertial_nav;
+
+    // Attitude, Position and Waypoint navigation objects
+    // To-Do: move inertial nav up or other navigation variables down here
+    // AC_AttitudeControl_t *attitude_control;
+    // AC_PosControl *pos_control;
+    // AC_WPNav *wp_nav;
+    // AC_Loiter *loiter_nav;
+
+
+    // System Timers
+    // --------------
+    // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
+    uint32_t arm_time_ms;
+
+    // Used to exit the roll and pitch auto trim function
+    uint8_t auto_trim_counter;
+    bool auto_trim_started = false;
+
+
+    // last valid RC input time
+    uint32_t last_radio_update_ms;
+
+    // Top-level logic
+    // setup the var_info table
+    AP_Param param_loader;
+
+    bool standby_active;
+
+    static const AP_Scheduler::Task scheduler_tasks[];
+    static const AP_Param::Info var_info[];
+    static const struct LogStructure log_structure[];
+
+    enum Failsafe_Action {
+        Failsafe_Action_None           = 0,
+        Failsafe_Action_Land           = 1,
+        Failsafe_Action_Terminate      = 5
+    };
+
+    enum class FailsafeOption {
+        RC_CONTINUE_IF_AUTO             = (1<<0),   // 1
+        GCS_CONTINUE_IF_AUTO            = (1<<1),   // 2
+        RC_CONTINUE_IF_GUIDED           = (1<<2),   // 4
+        CONTINUE_IF_LANDING             = (1<<3),   // 8
+        GCS_CONTINUE_IF_PILOT_CONTROL   = (1<<4),   // 16
+        RELEASE_GRIPPER                 = (1<<5),   // 32
+    };
+
+    static constexpr int8_t _failsafe_priorities[] = {
+        Failsafe_Action_Terminate,
+        Failsafe_Action_Land,
+        Failsafe_Action_None,
+        -1 // the priority list must end with a sentinel of -1
+    };
+
+#define FAILSAFE_LAND_PRIORITY 1
+    static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == Failsafe_Action_Land,
+                  "FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities");
+    static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
+                  "_failsafe_priorities is missing the sentinel");
+
+    // AP_State.cpp
+    void set_auto_armed(bool b);
+    void set_failsafe_radio(bool b);
+    void set_failsafe_gcs(bool b);
+    // void update_using_interlock();
+
+    // Blimp.cpp
+    void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
+                             uint8_t &task_count,
+                             uint32_t &log_bit) override;
+    void fast_loop() override;
+    // bool start_takeoff(float alt) override;
+    // bool set_target_location(const Location& target_loc) override;
+    // bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    // bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
+    void rc_loop();
+    void throttle_loop();
+    void update_batt_compass(void);
+    void fourhundred_hz_logging();
+    void ten_hz_logging_loop();
+    void twentyfive_hz_logging();
+    void three_hz_loop();
+    void one_hz_loop();
+    void read_AHRS(void);
+    void update_altitude();
+
+    // Attitude.cpp
+    float get_pilot_desired_yaw_rate(int16_t stick_angle);
+    // void update_throttle_hover();
+    float get_pilot_desired_climb_rate(float throttle_control);
+    float get_non_takeoff_throttle();
+    // void set_accel_throttle_I_from_pilot_throttle();
+    void rotate_body_frame_to_NE(float &x, float &y);
+    uint16_t get_pilot_speed_dn();
+
+    // baro_ground_effect.cpp
+    // void update_ground_effect_detector(void);
+    // void update_ekf_terrain_height_stable();
+
+    // commands.cpp
+    void update_home_from_EKF();
+    void set_home_to_current_location_inflight();
+    bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
+    bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
+    bool far_from_EKF_origin(const Location& loc);
+
+    // compassmot.cpp
+    // MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan);
+
+    // // crash_check.cpp
+    // void crash_check();
+    // void thrust_loss_check();
+    // void parachute_check();
+    // void parachute_release();
+    // void parachute_manual_release();
+
+    // ekf_check.cpp
+    void ekf_check();
+    bool ekf_over_threshold();
+    void failsafe_ekf_event();
+    void failsafe_ekf_off_event(void);
+    void check_ekf_reset();
+    void check_vibration();
+
+    // events.cpp
+    bool failsafe_option(FailsafeOption opt) const;
+    void failsafe_radio_on_event();
+    void failsafe_radio_off_event();
+    void handle_battery_failsafe(const char* type_str, const int8_t action);
+    void failsafe_gcs_check();
+    // void failsafe_gcs_on_event(void); //MIR will probably need these two soon.
+    // void failsafe_gcs_off_event(void);
+    // void gpsglitch_check();
+    // void set_mode_RTL_or_land_with_pause(ModeReason reason);
+    bool should_disarm_on_failsafe();
+    void do_failsafe_action(Failsafe_Action action, ModeReason reason);
+
+    // failsafe.cpp
+    void failsafe_enable();
+    void failsafe_disable();
+
+    // fence.cpp
+    void fence_check();
+
+    // inertia.cpp
+    void read_inertia();
+
+    // landing_detector.cpp
+    void update_land_and_crash_detectors();
+    void update_land_detector();
+    void set_land_complete(bool b);
+    void set_land_complete_maybe(bool b);
+    // void update_throttle_mix();
+
+    // landing_gear.cpp
+    void landinggear_update();
+
+    // // standby.cpp
+    // void standby_update();
+
+    // Log.cpp
+    void Log_Write_Control_Tuning();
+    void Log_Write_Performance();
+    void Log_Write_Attitude();
+    void Log_Write_EKF_POS();
+    void Log_Write_MotBatt();
+    void Log_Write_Data(LogDataID id, int32_t value);
+    void Log_Write_Data(LogDataID id, uint32_t value);
+    void Log_Write_Data(LogDataID id, int16_t value);
+    void Log_Write_Data(LogDataID id, uint16_t value);
+    void Log_Write_Data(LogDataID id, float value);
+    void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max);
+    void Log_Sensor_Health();
+    void Log_Write_Precland();
+    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
+    void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
+    void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
+    void Log_Write_Vehicle_Startup_Messages();
+    void log_init(void);
+
+    // mode.cpp
+    bool set_mode(Mode::Number mode, ModeReason reason);
+    bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
+    uint8_t get_mode() const override
+    {
+        return (uint8_t)control_mode;
+    }
+    void update_flight_mode();
+    void notify_flight_mode();
+
+    // mode_land.cpp
+    void set_mode_land_with_pause(ModeReason reason);
+    bool landing_with_GPS();
+
+    // // motors.cpp
+    void arm_motors_check();
+    // void auto_disarm_check();
+    void motors_output();
+    // void lost_vehicle_check();
+
+    // navigation.cpp
+    // void run_nav_updates(void);
+    // int32_t home_bearing();
+    // uint32_t home_distance();
+
+    // Parameters.cpp
+    void load_parameters(void) override;
+    void convert_pid_parameters(void);
+    void convert_lgr_parameters(void);
+    void convert_fs_options_params(void);
+
+    // radio.cpp
+    void default_dead_zones();
+    void init_rc_in();
+    void init_rc_out();
+    void enable_motor_output();
+    void read_radio();
+    void set_throttle_and_failsafe(uint16_t throttle_pwm);
+    void set_throttle_zero_flag(int16_t throttle_control);
+    // void radio_passthrough_to_motors();
+    int16_t get_throttle_mid(void);
+
+    // sensors.cpp
+    void read_barometer(void);
+    void init_rangefinder(void);
+    void read_rangefinder(void);
+    bool rangefinder_alt_ok();
+    bool rangefinder_up_ok();
+    void rpm_update();
+    void init_optflow();
+    void update_optical_flow(void);
+    void compass_cal_update(void);
+    void accel_cal_update(void);
+    void init_proximity();
+    void update_proximity();
+
+    // RC_Channel.cpp
+    void save_trim();
+    void auto_trim();
+    void auto_trim_cancel();
+
+    // system.cpp
+    void init_ardupilot() override;
+    void startup_INS_ground();
+    // void update_dynamic_notch() override;
+    bool position_ok() const;
+    bool ekf_has_absolute_position() const;
+    bool ekf_has_relative_position() const;
+    bool ekf_alt_ok() const;
+    void update_auto_armed();
+    bool should_log(uint32_t mask);
+    MAV_TYPE get_frame_mav_type();
+    const char* get_frame_string();
+    void allocate_motors(void);
+
+    // // tuning.cpp
+    // void tuning();
+
+    // vehicle specific waypoint info helpers
+    bool get_wp_distance_m(float &distance) const override;
+    bool get_wp_bearing_deg(float &bearing) const override;
+    bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
+
+    Mode *flightmode;
+    ModeManual mode_manual;
+    ModeLand mode_land;
+
+    // mode.cpp
+    Mode *mode_from_mode_num(const Mode::Number mode);
+    void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
+
+public:
+    void failsafe_check();      // failsafe.cpp
+};
+
+extern Blimp blimp;
+
+using AP_HAL::millis;
+using AP_HAL::micros;

Blimp/Fins.cpp

@@ -0,0 +1,153 @@
+#include "Blimp.h"
+
+//most of these will become parameters...
+//put here instead of Fins.h so that it can be changed without having to recompile the entire vehicle
+
+#define FIN_SCALE_MAX 1000
+
+/*
+  2nd group of parameters
+ */
+const AP_Param::GroupInfo Fins::var_info[] = {
+
+    // @Param: FREQ_HZ
+    // @DisplayName: Fins frequency
+    // @Description: This is the oscillation frequency of the fins
+    // @Range: 1 10
+    // @User: Standard
+    AP_GROUPINFO("FREQ_HZ", 1, Fins, freq_hz, 3),
+
+    // @Param: TURBO_MODE
+    // @DisplayName: Enable turbo mode
+    // @Description: Enables double speed on high offset.
+    // @Range: 0 1
+    // @User: Standard
+    AP_GROUPINFO("TURBO_MODE", 2, Fins, turbo_mode, 0),
+
+    AP_GROUPEND
+};
+
+//constructor
+Fins::Fins(uint16_t loop_rate) :
+    _loop_rate(loop_rate)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void Fins::setup_fins()
+{
+    //amp r   f   d     y,off r   f   d      y               right, front, down, yaw
+    add_fin(0,  0, -1, 0.5,   0,    0,  0, -0.5,    0); //Back(?)
+    add_fin(1,  0,  1, 0.5,   0,    0,  0, -0.5,    0); //Front(?)
+    add_fin(2, -1,  0,   0, 0.5,    0,  0,    0,  0.5); //Right
+    add_fin(3,  1,  0,   0, 0.5,    0,  0,    0, -0.5); //Left
+
+    SRV_Channels::set_angle(SRV_Channel::k_motor1, FIN_SCALE_MAX);
+    SRV_Channels::set_angle(SRV_Channel::k_motor2, FIN_SCALE_MAX);
+    SRV_Channels::set_angle(SRV_Channel::k_motor3, FIN_SCALE_MAX);
+    SRV_Channels::set_angle(SRV_Channel::k_motor4, FIN_SCALE_MAX);
+
+    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MIR: All fins have been added.");
+}
+
+void Fins::add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float down_amp_fac, float yaw_amp_fac,
+                   float right_off_fac, float front_off_fac, float down_off_fac, float yaw_off_fac)
+{
+
+    // ensure valid fin number is provided
+    if (fin_num >= 0 && fin_num < NUM_FINS) {
+
+        // set amplitude factors
+        _right_amp_factor[fin_num] = right_amp_fac;
+        _front_amp_factor[fin_num] = front_amp_fac;
+        _down_amp_factor[fin_num] = down_amp_fac;
+        _yaw_amp_factor[fin_num] = yaw_amp_fac;
+
+        // set offset factors
+        _right_off_factor[fin_num] = right_off_fac;
+        _front_off_factor[fin_num] = front_off_fac;
+        _down_off_factor[fin_num] = down_off_fac;
+        _yaw_off_factor[fin_num] = yaw_off_fac;
+    }
+}
+
+//B,F,R,L = 0,1,2,3
+void Fins::output()
+{
+    if (!_armed) {
+        // set everything to zero so fins stop moving
+        right_out = 0;
+        front_out = 0;
+        down_out  = 0;
+        yaw_out   = 0;
+    }
+
+    right_out /= RC_SCALE;
+    front_out /= RC_SCALE;
+    down_out  /= RC_SCALE;
+    yaw_out   /= RC_SCALE;
+
+    _time = AP_HAL::micros() * 1.0e-6;
+
+    for (int8_t i=0; i<NUM_FINS; i++) {
+        _amp[i] =  max(0,_right_amp_factor[i]*right_out) + max(0,_front_amp_factor[i]*front_out) +
+                   fabsf(_down_amp_factor[i]*down_out) + fabsf(_yaw_amp_factor[i]*yaw_out);
+        _off[i] = _right_off_factor[i]*right_out + _front_off_factor[i]*front_out +
+                  _down_off_factor[i]*down_out + _yaw_off_factor[i]*yaw_out;
+        _omm[i] = 1;
+
+        _num_added = 0;
+        if (max(0,_right_amp_factor[i]*right_out) > 0.0f) {
+            _num_added++;
+        }
+        if (max(0,_front_amp_factor[i]*front_out) > 0.0f) {
+            _num_added++;
+        }
+        if (fabsf(_down_amp_factor[i]*down_out) > 0.0f) {
+            _num_added++;
+        }
+        if (fabsf(_yaw_amp_factor[i]*yaw_out) > 0.0f) {
+            _num_added++;
+        }
+
+        if (_num_added > 0) {
+            _off[i] = _off[i]/_num_added; //average the offsets
+        }
+
+        if ((_amp[i]+fabsf(_off[i])) > 1) {
+            _amp[i] = 1 - fabsf(_off[i]);
+        }
+
+        if (turbo_mode) {
+            //double speed fins if offset at max... MIR
+            if (_amp[i] <= 0.6 && fabsf(_off[i]) >= 0.4) {
+                _omm[i] = 2;
+            }
+        }
+
+        // finding and outputting current position for each servo from sine wave
+        _pos[i]= _amp[i]*cosf(freq_hz * _omm[i] * _time * 2 * M_PI) + _off[i]; //removed +MAX_AMP because output can do -ve numbers
+        SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX);
+    }
+
+    ::printf("FINS (amp %.1f %.1f %.1f %.1f   off %.1f %.1f %.1f %.1f   omm %.1f %.1f %.1f %.1f)\n",
+             _amp[0], _amp[1], _amp[2], _amp[3], _off[0], _off[1], _off[2], _off[3], _omm[0], _omm[1], _omm[2], _omm[3]);
+}
+
+void Fins::output_min()
+{
+    right_out = 0;
+    front_out = 0;
+    down_out  = 0;
+    yaw_out   = 0;
+    Fins::output();
+}
+
+// MIR - Probably want to completely get rid of the desired spool state thing.
+void Fins::set_desired_spool_state(DesiredSpoolState spool)
+{
+    if (_armed || (spool == DesiredSpoolState::SHUT_DOWN)) {
+        // Set DesiredSpoolState only if it is either armed or it wants to shut down.
+        _spool_desired = spool;
+    }
+};

Blimp/Fins.h

@@ -0,0 +1,175 @@
+//This class converts horizontal acceleration commands to fin flapping commands.
+#pragma once
+// #include <AP_Common/AP_Common.h>
+// #include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Notify/AP_Notify.h>      // Notify library
+#include <SRV_Channel/SRV_Channel.h>
+// #include <Filter/Filter.h>         // filter library
+// #include <AP_HAL/AP_HAL.h>
+// #include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+// #define FINS_SPEED_DEFAULT 10 //MIR what is this?
+#define NUM_FINS 4
+#define RC_SCALE 1000
+class Fins
+{
+public:
+    friend class Blimp;
+    // Fins(void);
+
+    enum motor_frame_class {
+        MOTOR_FRAME_UNDEFINED = 0,
+        MOTOR_FRAME_AIRFISH = 1,
+    };
+    enum motor_frame_type {
+        MOTOR_FRAME_TYPE_AIRFISH = 1,
+    };
+
+    //constructor
+    Fins(uint16_t loop_rate);
+
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo        var_info[];
+
+    // singleton support
+    // static Fins    *get_singleton(void) { return _singleton; }
+
+    // desired spool states
+    // from AP_Motors_Class.h
+    enum class DesiredSpoolState : uint8_t {
+        SHUT_DOWN = 0,              // all motors should move to stop
+        // GROUND_IDLE = 1,            // all motors should move to ground idle
+        THROTTLE_UNLIMITED = 2,     // motors should move to being a state where throttle is unconstrained (e.g. by start up procedure)
+    };
+
+    // spool states
+    enum class SpoolState : uint8_t {
+        SHUT_DOWN = 0,                      // all motors stop
+        // GROUND_IDLE = 1,                    // all motors at ground idle
+        // SPOOLING_UP = 2,                       // increasing maximum throttle while stabilizing
+        THROTTLE_UNLIMITED = 3,             // throttle is no longer constrained by start up procedure
+        // SPOOLING_DOWN = 4,                     // decreasing maximum throttle while stabilizing
+    };
+
+    bool initialised_ok() const
+    {
+        return true;
+    }
+
+    void armed(bool arm)
+    {
+        if (arm != _armed) {
+            _armed = arm;
+            AP_Notify::flags.armed = arm;
+        }
+
+    }
+    bool armed() const
+    {
+        return _armed;
+    }
+
+protected:
+    // internal variables
+    const uint16_t      _loop_rate;                 // rate in Hz at which output() function is called (normally 400hz)
+    uint16_t            _speed_hz;                  // speed in hz to send updates to motors
+    // float               _roll_in;                   // desired roll control from attitude controllers, -1 ~ +1
+    // float               _roll_in_ff;                // desired roll feed forward control from attitude controllers, -1 ~ +1
+    // float               _pitch_in;                  // desired pitch control from attitude controller, -1 ~ +1
+    // float               _pitch_in_ff;               // desired pitch feed forward control from attitude controller, -1 ~ +1
+    // float               _yaw_in;                    // desired yaw control from attitude controller, -1 ~ +1
+    // float               _yaw_in_ff;                 // desired yaw feed forward control from attitude controller, -1 ~ +1
+    float               _throttle_in;               // last throttle input from set_throttle caller
+    float               _down_out;                  // throttle after mixing is complete
+    // float               _forward_in;                // last forward input from set_forward caller
+    // float               _lateral_in;                // last lateral input from set_lateral caller
+    float               _throttle_avg_max;          // last throttle input from set_throttle_avg_max
+    // LowPassFilterFloat  _throttle_filter;           // throttle input filter
+    DesiredSpoolState   _spool_desired;             // desired spool state
+    SpoolState          _spool_state;               // current spool mode
+
+    float               _air_density_ratio;         //air density as a proportion of sea level density
+
+    float               _time;                       //current timestep
+
+    bool _armed;             // 0 if disarmed, 1 if armed
+
+    float              _amp[NUM_FINS]; //amplitudes
+    float              _off[NUM_FINS]; //offsets
+    float              _omm[NUM_FINS]; //omega multiplier
+    float              _pos[NUM_FINS]; //servo positions
+
+    float               _right_amp_factor[NUM_FINS];
+    float               _front_amp_factor[NUM_FINS];
+    float               _down_amp_factor[NUM_FINS];
+    float               _yaw_amp_factor[NUM_FINS];
+
+    float               _right_off_factor[NUM_FINS];
+    float               _front_off_factor[NUM_FINS];
+    float               _down_off_factor[NUM_FINS];
+    float               _yaw_off_factor[NUM_FINS];
+
+    int8_t              _num_added;
+    // private:
+public:
+    float               right_out;                  //input right movement, negative for left, +1 to -1
+    float               front_out;                  //input front/forwards movement, negative for backwards, +1 to -1
+    float               yaw_out;                    //input yaw, +1 to -1
+    float               down_out;                   //input height control, +1 to -1
+
+    AP_Float            freq_hz;
+    AP_Int8             turbo_mode;
+
+    bool _interlock;         // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run)
+    bool _initialised_ok;    // 1 if initialisation was successful
+
+    // get_spool_state - get current spool state
+    enum SpoolState  get_spool_state(void) const
+    {
+        return _spool_state;
+    }
+
+    // float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
+    //     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; }
+
+    float max(float one, float two)
+    {
+        if (one >= two) {
+            return one;
+        } else {
+            return two;
+        }
+    }
+
+    void output_min();
+
+    void add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float yaw_amp_fac, float down_amp_fac,
+                 float right_off_fac, float front_off_fac, float yaw_off_fac, float down_off_fac);
+
+    void setup_fins();
+
+    float get_throttle_hover()
+    {
+        return 0;    //MIR temp
+    }
+
+    void set_desired_spool_state(DesiredSpoolState spool);
+
+    void output();
+
+    float get_throttle()
+    {
+        return 0.1f;    //MIR temp
+    }
+
+    void rc_write(uint8_t chan, uint16_t pwm);
+
+    // set_density_ratio - sets air density as a proportion of sea level density
+    void  set_air_density_ratio(float ratio)
+    {
+        _air_density_ratio = ratio;
+    }
+
+};

Blimp/GCS_Blimp.cpp

@@ -0,0 +1,69 @@
+#include "GCS_Blimp.h"
+
+#include "Blimp.h"
+
+uint8_t GCS_Blimp::sysid_this_mav() const
+{
+    return blimp.g.sysid_this_mav;
+}
+
+const char* GCS_Blimp::frame_string() const
+{
+    return blimp.get_frame_string();
+}
+
+void GCS_Blimp::update_vehicle_sensor_status_flags(void)
+{
+    control_sensors_present |=
+        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
+        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
+        MAV_SYS_STATUS_SENSOR_YAW_POSITION;
+
+    control_sensors_enabled |=
+        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
+        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
+        MAV_SYS_STATUS_SENSOR_YAW_POSITION;
+
+    control_sensors_health |=
+        MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
+        MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
+        MAV_SYS_STATUS_SENSOR_YAW_POSITION;
+
+    const Blimp::ap_t &ap = blimp.ap;
+
+    if (ap.rc_receiver_present) {
+        control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
+    }
+
+    control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+    control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
+
+    // switch (blimp.control_mode) {
+    // case Mode::Number::AUTO:
+    // case Mode::Number::AVOID_ADSB:
+    // case Mode::Number::GUIDED:
+    // case Mode::Number::LOITER:
+    //     control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+    //     control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+    //     control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
+    //     control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
+    //     break;
+    // case Mode::Number::ALT_HOLD:
+    // case Mode::Number::GUIDED_NOGPS:
+    // case Mode::Number::SPORT:
+    // case Mode::Number::AUTOTUNE:
+    // case Mode::Number::FLOWHOLD:
+    //     control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+    //     control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+    //     break;
+    // default:
+    //     // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)
+    //     break;
+    // }
+
+    if (ap.rc_receiver_present && !blimp.failsafe.radio) {
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
+    }
+
+}

Blimp/GCS_Blimp.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <GCS_MAVLink/GCS.h>
+#include "GCS_Mavlink.h"
+
+class GCS_Blimp : public GCS
+{
+    friend class Blimp; // for access to _chan in parameter declarations
+
+public:
+
+    // return GCS link at offset ofs
+    GCS_MAVLINK_Blimp *chan(const uint8_t ofs) override
+    {
+        if (ofs > _num_gcs) {
+            INTERNAL_ERROR(AP_InternalError::error_t::gcs_offset);
+            return nullptr;
+        }
+        return (GCS_MAVLINK_Blimp *)_chan[ofs];
+    }
+    const GCS_MAVLINK_Blimp *chan(const uint8_t ofs) const override
+    {
+        if (ofs > _num_gcs) {
+            INTERNAL_ERROR(AP_InternalError::error_t::gcs_offset);
+            return nullptr;
+        }
+        return (GCS_MAVLINK_Blimp *)_chan[ofs];
+    }
+
+    void update_vehicle_sensor_status_flags(void) override;
+
+    uint32_t custom_mode() const override;
+    MAV_TYPE frame_type() const override;
+
+    const char* frame_string() const override;
+
+    bool vehicle_initialised() const override;
+
+protected:
+
+    uint8_t sysid_this_mav() const override;
+
+    // minimum amount of time (in microseconds) that must remain in
+    // the main scheduler loop before we are allowed to send any
+    // mavlink messages.  We want to prioritise the main flight
+    // control loop over communications
+    uint16_t min_loop_time_remaining_for_message_send_us() const override
+    {
+        return 250;
+    }
+
+    GCS_MAVLINK_Blimp *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
+            AP_HAL::UARTDriver &uart) override
+    {
+        return new GCS_MAVLINK_Blimp(params, uart);
+    }
+
+};

Blimp/GCS_Mavlink.cpp

@@ -0,0 +1,977 @@
+#include "Blimp.h"
+
+#include "GCS_Mavlink.h"
+
+/*
+ *  !!NOTE!!
+ *
+ *  the use of NOINLINE separate functions for each message type avoids
+ *  a compiler bug in gcc that would cause it to use far more stack
+ *  space than is needed. Without the NOINLINE we use the sum of the
+ *  stack needed for each message type. Please be careful to follow the
+ *  pattern below when adding any new messages
+ */
+
+MAV_TYPE GCS_Blimp::frame_type() const
+{
+    return blimp.get_frame_mav_type();
+}
+
+MAV_MODE GCS_MAVLINK_Blimp::base_mode() const
+{
+    uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
+    // work out the base_mode. This value is not very useful
+    // for APM, but we calculate it as best we can so a generic
+    // MAVLink enabled ground station can work out something about
+    // what the MAV is up to. The actual bit values are highly
+    // ambiguous for most of the APM flight modes. In practice, you
+    // only get useful information from the custom_mode, which maps to
+    // the APM flight mode and has a well defined meaning in the
+    // ArduPlane documentation
+
+    // switch (blimp.control_mode) {
+    // case Mode::Number::AUTO:
+    // case Mode::Number::RTL:
+    // case Mode::Number::LOITER:
+    // case Mode::Number::AVOID_ADSB:
+    // case Mode::Number::FOLLOW:
+    // case Mode::Number::GUIDED:
+    // case Mode::Number::CIRCLE:
+    // case Mode::Number::POSHOLD:
+    // case Mode::Number::BRAKE:
+    // case Mode::Number::SMART_RTL:
+    //     _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+    //     // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
+    //     // APM does in any mode, as that is defined as "system finds its own goal
+    //     // positions", which APM does not currently do
+    //     break;
+    // default:
+    //     break;
+    // }
+
+    // all modes except INITIALISING have some form of manual
+    // override if stick mixing is enabled
+    _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+
+#if HIL_MODE != HIL_MODE_DISABLED
+    _base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+#endif
+
+    // we are armed if we are not initialising
+    if (blimp.motors != nullptr && blimp.motors->armed()) {
+        _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+    }
+
+    // indicate we have set a custom mode
+    _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+    return (MAV_MODE)_base_mode;
+}
+
+uint32_t GCS_Blimp::custom_mode() const
+{
+    return (uint32_t)blimp.control_mode;
+}
+
+MAV_STATE GCS_MAVLINK_Blimp::vehicle_system_status() const
+{
+    // set system as critical if any failsafe have triggered
+    if (blimp.any_failsafe_triggered())  {
+        return MAV_STATE_CRITICAL;
+    }
+
+    if (blimp.ap.land_complete) {
+        return MAV_STATE_STANDBY;
+    }
+
+    return MAV_STATE_ACTIVE;
+}
+
+
+void GCS_MAVLINK_Blimp::send_position_target_global_int()
+{
+    Location target;
+    if (!blimp.flightmode->get_wp(target)) {
+        return;
+    }
+    mavlink_msg_position_target_global_int_send(
+        chan,
+        AP_HAL::millis(), // time_boot_ms
+        MAV_FRAME_GLOBAL, // targets are always global altitude
+        0xFFF8, // ignore everything except the x/y/z components
+        target.lat, // latitude as 1e7
+        target.lng, // longitude as 1e7
+        target.alt * 0.01f, // altitude is sent as a float
+        0.0f, // vx
+        0.0f, // vy
+        0.0f, // vz
+        0.0f, // afx
+        0.0f, // afy
+        0.0f, // afz
+        0.0f, // yaw
+        0.0f); // yaw_rate
+}
+
+// void GCS_MAVLINK_Blimp::send_position_target_local_ned()
+// {
+// #if MODE_GUIDED_ENABLED == ENABLED
+//     if (!blimp.flightmode->in_guided_mode()) {
+//         return;
+//     }
+
+//     const GuidedMode guided_mode = blimp.mode_guided.mode();
+//     Vector3f target_pos;
+//     Vector3f target_vel;
+//     uint16_t type_mask;
+
+//     if (guided_mode == Guided_WP) {
+//         type_mask = 0x0FF8; // ignore everything except position
+//         target_pos = blimp.wp_nav->get_wp_destination() * 0.01f; // convert to metres
+//     } else if (guided_mode == Guided_Velocity) {
+//         type_mask = 0x0FC7; // ignore everything except velocity
+//         target_vel = blimp.flightmode->get_desired_velocity() * 0.01f; // convert to m/s
+//     } else {
+//         type_mask = 0x0FC0; // ignore everything except position & velocity
+//         target_pos = blimp.wp_nav->get_wp_destination() * 0.01f;
+//         target_vel = blimp.flightmode->get_desired_velocity() * 0.01f;
+//     }
+
+//     mavlink_msg_position_target_local_ned_send(
+//         chan,
+//         AP_HAL::millis(), // time boot ms
+//         MAV_FRAME_LOCAL_NED,
+//         type_mask,
+//         target_pos.x, // x in metres
+//         target_pos.y, // y in metres
+//         -target_pos.z, // z in metres NED frame
+//         target_vel.x, // vx in m/s
+//         target_vel.y, // vy in m/s
+//         -target_vel.z, // vz in m/s NED frame
+//         0.0f, // afx
+//         0.0f, // afy
+//         0.0f, // afz
+//         0.0f, // yaw
+//         0.0f); // yaw_rate
+// #endif
+// }
+
+void GCS_MAVLINK_Blimp::send_nav_controller_output() const
+{
+    //     if (!blimp.ap.initialised) {
+    //         return;
+    //     }
+    //     const Vector3f &targets = blimp.attitude_control->get_att_target_euler_cd();
+    //     const Mode *flightmode = blimp.flightmode;
+    //     mavlink_msg_nav_controller_output_send(
+    //         chan,
+    //         targets.x * 1.0e-2f,
+    //         targets.y * 1.0e-2f,
+    //         targets.z * 1.0e-2f,
+    //         flightmode->wp_bearing() * 1.0e-2f,
+    //         MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX),
+    //         blimp.pos_control->get_alt_error() * 1.0e-2f,
+    //         0,
+    //         flightmode->crosstrack_error() * 1.0e-2f);
+}
+
+float GCS_MAVLINK_Blimp::vfr_hud_airspeed() const
+{
+    Vector3f airspeed_vec_bf;
+    if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
+        // we are running the EKF3 wind estimation code which can give
+        // us an airspeed estimate
+        return airspeed_vec_bf.length();
+    }
+    return AP::gps().ground_speed();
+}
+
+int16_t GCS_MAVLINK_Blimp::vfr_hud_throttle() const
+{
+    if (blimp.motors == nullptr) {
+        return 0;
+    }
+    return (int16_t)(blimp.motors->get_throttle() * 100);
+}
+
+/*
+  send PID tuning message
+ */
+void GCS_MAVLINK_Blimp::send_pid_tuning()
+{
+    static const PID_TUNING_AXIS axes[] = {
+        PID_TUNING_ROLL,
+        PID_TUNING_PITCH,
+        PID_TUNING_YAW,
+        PID_TUNING_ACCZ
+    };
+    for (uint8_t i=0; i<ARRAY_SIZE(axes); i++) {
+        if (!(blimp.g.gcs_pid_mask & (1<<(axes[i]-1)))) {
+            continue;
+        }
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+        const AP_Logger::PID_Info *pid_info = nullptr;
+        switch (axes[i]) { //MIR Temp
+        // case PID_TUNING_ROLL:
+        //     pid_info = &blimp.attitude_control->get_rate_roll_pid().get_pid_info();
+        //     break;
+        // case PID_TUNING_PITCH:
+        //     pid_info = &blimp.attitude_control->get_rate_pitch_pid().get_pid_info();
+        //     break;
+        // case PID_TUNING_YAW:
+        //     pid_info = &blimp.attitude_control->get_rate_yaw_pid().get_pid_info();
+        //     break;
+        // case PID_TUNING_ACCZ:
+        //     pid_info = &blimp.pos_control->get_accel_z_pid().get_pid_info();
+        //     break;
+        default:
+            continue;
+        }
+        if (pid_info != nullptr) {
+            mavlink_msg_pid_tuning_send(chan,
+                                        axes[i],
+                                        pid_info->target,
+                                        pid_info->actual,
+                                        pid_info->FF,
+                                        pid_info->P,
+                                        pid_info->I,
+                                        pid_info->D);
+        }
+    }
+}
+
+uint8_t GCS_MAVLINK_Blimp::sysid_my_gcs() const
+{
+    return blimp.g.sysid_my_gcs;
+}
+bool GCS_MAVLINK_Blimp::sysid_enforce() const
+{
+    return blimp.g2.sysid_enforce;
+}
+
+uint32_t GCS_MAVLINK_Blimp::telem_delay() const
+{
+    return (uint32_t)(blimp.g.telem_delay);
+}
+
+bool GCS_Blimp::vehicle_initialised() const
+{
+    return blimp.ap.initialised;
+}
+
+// try to send a message, return false if it wasn't sent
+bool GCS_MAVLINK_Blimp::try_send_message(enum ap_message id)
+{
+    switch (id) {
+
+    case MSG_WIND:
+        CHECK_PAYLOAD_SIZE(WIND);
+        send_wind();
+        break;
+
+    case MSG_SERVO_OUT:
+    case MSG_AOA_SSA:
+    case MSG_LANDING:
+    case MSG_ADSB_VEHICLE:
+        // unused
+        break;
+
+    default:
+        return GCS_MAVLINK::try_send_message(id);
+    }
+    return true;
+}
+
+
+const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
+    // @Param: RAW_SENS
+    // @DisplayName: Raw sensor stream rate
+    // @Description: Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3 and SENSOR_OFFSETS to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0],  0),
+
+    // @Param: EXT_STAT
+    // @DisplayName: Extended status stream rate to ground station
+    // @Description: Stream rate of SYS_STATUS, POWER_STATUS, MEMINFO, CURRENT_WAYPOINT, GPS_RAW_INT, GPS_RTK (if available), GPS2_RAW (if available), GPS2_RTK (if available), NAV_CONTROLLER_OUTPUT, and FENCE_STATUS to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1],  0),
+
+    // @Param: RC_CHAN
+    // @DisplayName: RC Channel stream rate to ground station
+    // @Description: Stream rate of SERVO_OUTPUT_RAW and RC_CHANNELS to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK_Parameters, streamRates[2],  0),
+
+    // @Param: RAW_CTRL
+    // @DisplayName: Raw Control stream rate to ground station
+    // @Description: Stream rate of RC_CHANNELS_SCALED (HIL only) to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3],  0),
+
+    // @Param: POSITION
+    // @DisplayName: Position stream rate to ground station
+    // @Description: Stream rate of GLOBAL_POSITION_INT and LOCAL_POSITION_NED to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4],  0),
+
+    // @Param: EXTRA1
+    // @DisplayName: Extra data type 1 stream rate to ground station
+    // @Description: Stream rate of ATTITUDE, SIMSTATE (SITL only), AHRS2 and PID_TUNING to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK_Parameters, streamRates[5],  0),
+
+    // @Param: EXTRA2
+    // @DisplayName: Extra data type 2 stream rate to ground station
+    // @Description: Stream rate of VFR_HUD to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK_Parameters, streamRates[6],  0),
+
+    // @Param: EXTRA3
+    // @DisplayName: Extra data type 3 stream rate to ground station
+    // @Description: Stream rate of AHRS, HWSTATUS, SYSTEM_TIME, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY2, MOUNT_STATUS, OPTICAL_FLOW, GIMBAL_REPORT, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION and RPM to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK_Parameters, streamRates[7],  0),
+
+    // @Param: PARAMS
+    // @DisplayName: Parameter stream rate to ground station
+    // @Description: Stream rate of PARAM_VALUE to ground station
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK_Parameters, streamRates[8],  0),
+    AP_GROUPEND
+};
+
+static const ap_message STREAM_RAW_SENSORS_msgs[] = {
+    MSG_RAW_IMU,
+    MSG_SCALED_IMU2,
+    MSG_SCALED_IMU3,
+    MSG_SCALED_PRESSURE,
+    MSG_SCALED_PRESSURE2,
+    MSG_SCALED_PRESSURE3,
+    MSG_SENSOR_OFFSETS
+};
+static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
+    MSG_SYS_STATUS,
+    MSG_POWER_STATUS,
+    MSG_MEMINFO,
+    MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
+    MSG_GPS_RAW,
+    MSG_GPS_RTK,
+    MSG_GPS2_RAW,
+    MSG_GPS2_RTK,
+    MSG_NAV_CONTROLLER_OUTPUT,
+    MSG_FENCE_STATUS,
+    MSG_POSITION_TARGET_GLOBAL_INT,
+};
+static const ap_message STREAM_POSITION_msgs[] = {
+    MSG_LOCATION,
+    MSG_LOCAL_POSITION
+};
+static const ap_message STREAM_RC_CHANNELS_msgs[] = {
+    MSG_SERVO_OUTPUT_RAW,
+    MSG_RC_CHANNELS,
+    MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
+};
+static const ap_message STREAM_EXTRA1_msgs[] = {
+    MSG_ATTITUDE,
+    MSG_SIMSTATE,
+    MSG_AHRS2,
+    MSG_PID_TUNING // Up to four PID_TUNING messages are sent, depending on GCS_PID_MASK parameter
+};
+static const ap_message STREAM_EXTRA2_msgs[] = {
+    MSG_VFR_HUD
+};
+static const ap_message STREAM_EXTRA3_msgs[] = {
+    MSG_AHRS,
+    MSG_HWSTATUS,
+    MSG_SYSTEM_TIME,
+    MSG_WIND,
+    MSG_RANGEFINDER,
+    MSG_DISTANCE_SENSOR,
+    MSG_BATTERY2,
+    MSG_BATTERY_STATUS,
+    MSG_MOUNT_STATUS,
+    MSG_OPTICAL_FLOW,
+    MSG_GIMBAL_REPORT,
+    MSG_MAG_CAL_REPORT,
+    MSG_MAG_CAL_PROGRESS,
+    MSG_EKF_STATUS_REPORT,
+    MSG_VIBRATION,
+    MSG_RPM,
+    MSG_ESC_TELEMETRY,
+    MSG_GENERATOR_STATUS,
+};
+static const ap_message STREAM_PARAMS_msgs[] = {
+    MSG_NEXT_PARAM
+};
+static const ap_message STREAM_ADSB_msgs[] = {
+    MSG_ADSB_VEHICLE
+};
+
+const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
+    MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
+    MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
+    MAV_STREAM_ENTRY(STREAM_POSITION),
+    MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
+    MAV_STREAM_ENTRY(STREAM_EXTRA1),
+    MAV_STREAM_ENTRY(STREAM_EXTRA2),
+    MAV_STREAM_ENTRY(STREAM_EXTRA3),
+    MAV_STREAM_ENTRY(STREAM_ADSB),
+    MAV_STREAM_ENTRY(STREAM_PARAMS),
+    MAV_STREAM_TERMINATOR // must have this at end of stream_entries
+};
+
+bool GCS_MAVLINK_Blimp::handle_guided_request(AP_Mission::Mission_Command &cmd)
+{
+    // #if MODE_AUTO_ENABLED == ENABLED
+    //     // return blimp.mode_auto.do_guided(cmd);
+    // #else
+    return false;
+    // #endif
+}
+
+void GCS_MAVLINK_Blimp::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
+{
+    // add home alt if needed
+    if (cmd.content.location.relative_alt) {
+        cmd.content.location.alt += blimp.ahrs.get_home().alt;
+    }
+
+    // To-Do: update target altitude for loiter or waypoint controller depending upon nav mode
+}
+
+void GCS_MAVLINK_Blimp::packetReceived(const mavlink_status_t &status,
+                                       const mavlink_message_t &msg)
+{
+    GCS_MAVLINK::packetReceived(status, msg);
+}
+
+bool GCS_MAVLINK_Blimp::params_ready() const
+{
+    if (AP_BoardConfig::in_config_error()) {
+        // we may never have parameters "initialised" in this case
+        return true;
+    }
+    // if we have not yet initialised (including allocating the motors
+    // object) we drop this request. That prevents the GCS from getting
+    // a confusing parameter count during bootup
+    return blimp.ap.initialised_params;
+}
+
+void GCS_MAVLINK_Blimp::send_banner()
+{
+    GCS_MAVLINK::send_banner();
+    send_text(MAV_SEVERITY_INFO, "Frame: %s", blimp.get_frame_string());
+}
+
+// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
+void GCS_MAVLINK_Blimp::handle_rc_channels_override(const mavlink_message_t &msg)
+{
+    blimp.failsafe.last_heartbeat_ms = AP_HAL::millis();
+    GCS_MAVLINK::handle_rc_channels_override(msg);
+}
+
+void GCS_MAVLINK_Blimp::handle_command_ack(const mavlink_message_t &msg)
+{
+    blimp.command_ack_counter++;
+    GCS_MAVLINK::handle_command_ack(msg);
+}
+
+MAV_RESULT GCS_MAVLINK_Blimp::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
+{
+    return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
+}
+
+
+MAV_RESULT GCS_MAVLINK_Blimp::handle_command_do_set_roi(const Location &roi_loc)
+{
+    if (!roi_loc.check_latlng()) {
+        return MAV_RESULT_FAILED;
+    }
+    // blimp.flightmode->auto_yaw.set_roi(roi_loc);
+    return MAV_RESULT_ACCEPTED;
+}
+
+MAV_RESULT GCS_MAVLINK_Blimp::handle_preflight_reboot(const mavlink_command_long_t &packet)
+{
+    // call parent
+    return GCS_MAVLINK::handle_preflight_reboot(packet);
+}
+
+bool GCS_MAVLINK_Blimp::set_home_to_current_location(bool _lock)
+{
+    return blimp.set_home_to_current_location(_lock);
+}
+bool GCS_MAVLINK_Blimp::set_home(const Location& loc, bool _lock)
+{
+    return blimp.set_home(loc, _lock);
+}
+
+MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
+{
+    const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
+    if (!blimp.flightmode->in_guided_mode() && !change_modes) {
+        return MAV_RESULT_DENIED;
+    }
+
+    // sanity check location
+    if (!check_latlng(packet.x, packet.y)) {
+        return MAV_RESULT_DENIED;
+    }
+
+    Location request_location {};
+    request_location.lat = packet.x;
+    request_location.lng = packet.y;
+
+    if (fabsf(packet.z) > LOCATION_ALT_MAX_M) {
+        return MAV_RESULT_DENIED;
+    }
+
+    Location::AltFrame frame;
+    if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, frame)) {
+        return MAV_RESULT_DENIED; // failed as the location is not valid
+    }
+    request_location.set_alt_cm((int32_t)(packet.z * 100.0f), frame);
+
+    if (request_location.sanitize(blimp.current_loc)) {
+        // if the location wasn't already sane don't load it
+        return MAV_RESULT_DENIED; // failed as the location is not valid
+    }
+
+    return MAV_RESULT_ACCEPTED;
+}
+
+MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_packet(const mavlink_command_int_t &packet)
+{
+    switch (packet.command) {
+    case MAV_CMD_DO_FOLLOW:
+        return MAV_RESULT_UNSUPPORTED;
+
+    case MAV_CMD_DO_REPOSITION:
+        return handle_command_int_do_reposition(packet);
+    default:
+        return GCS_MAVLINK::handle_command_int_packet(packet);
+    }
+}
+
+MAV_RESULT GCS_MAVLINK_Blimp::handle_command_mount(const mavlink_command_long_t &packet)
+{
+    // if the mount doesn't do pan control then yaw the entire vehicle instead:
+    switch (packet.command) {
+    default:
+        break;
+    }
+    return GCS_MAVLINK::handle_command_mount(packet);
+}
+
+MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet)
+{
+    switch (packet.command) {
+
+    case MAV_CMD_NAV_TAKEOFF: {
+        // param3 : horizontal navigation by pilot acceptable
+        // param4 : yaw angle   (not supported)
+        // param5 : latitude    (not supported)
+        // param6 : longitude   (not supported)
+        // param7 : altitude [metres]
+
+        // float takeoff_alt = packet.param7 * 100;      // Convert m to cm
+
+        // if (!blimp.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {
+        //     return MAV_RESULT_FAILED;
+        //MIR Do I need this?
+        // }
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    // #if MODE_AUTO_ENABLED == ENABLED
+    //     case MAV_CMD_DO_LAND_START:
+    //         if (blimp.mode_auto.mission.jump_to_landing_sequence() && blimp.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) {
+    //             return MAV_RESULT_ACCEPTED;
+    //         }
+    //         return MAV_RESULT_FAILED;
+    // #endif
+
+    // case MAV_CMD_NAV_LOITER_UNLIM:
+    //     if (!blimp.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
+    //         return MAV_RESULT_FAILED;
+    //     }
+    //     return MAV_RESULT_ACCEPTED;
+
+    // case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+    //     if (!blimp.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) {
+    //         return MAV_RESULT_FAILED;
+    //     }
+    //     return MAV_RESULT_ACCEPTED;
+
+
+    case MAV_CMD_CONDITION_YAW:
+        // param1 : target angle [0-360]
+        // param2 : speed during change [deg per second]
+        // param3 : direction (-1:ccw, +1:cw)
+        // param4 : relative offset (1) or absolute angle (0)
+        if ((packet.param1 >= 0.0f)   &&
+            (packet.param1 <= 360.0f) &&
+            (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
+            // blimp.flightmode->auto_yaw.set_fixed_yaw(
+            // packet.param1,
+            // packet.param2,
+            // (int8_t)packet.param3,
+            // is_positive(packet.param4));
+            return MAV_RESULT_ACCEPTED;
+        }
+        return MAV_RESULT_FAILED;
+
+    default:
+        return GCS_MAVLINK::handle_command_long_packet(packet);
+    }
+}
+
+void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg)
+{
+    switch (msg.msgid) {
+
+    case MAVLINK_MSG_ID_HEARTBEAT: {    // MAV ID: 0
+        // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
+        if (msg.sysid != blimp.g.sysid_my_gcs) {
+            break;
+        }
+        blimp.failsafe.last_heartbeat_ms = AP_HAL::millis();
+        break;
+    }
+
+    // #if MODE_GUIDED_ENABLED == ENABLED
+    //     case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
+    //     {
+    //         // decode packet
+    //         mavlink_set_attitude_target_t packet;
+    //         mavlink_msg_set_attitude_target_decode(&msg, &packet);
+
+    //         // exit if vehicle is not in Guided mode or Auto-Guided mode
+    //         if (!blimp.flightmode->in_guided_mode()) {
+    //             break;
+    //         }
+
+    //         // ensure type_mask specifies to use attitude and thrust
+    //         if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
+    //             break;
+    //         }
+
+    //         // check if the message's thrust field should be interpreted as a climb rate or as thrust
+    //         const bool use_thrust = blimp.g2.dev_options.get() & DevOptionSetAttitudeTarget_ThrustAsThrust;
+
+    //         float climb_rate_or_thrust;
+    //         if (use_thrust) {
+    //             // interpret thrust as thrust
+    //             climb_rate_or_thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
+    //         } else {
+    //             // convert thrust to climb rate
+    //             packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
+    //             if (is_equal(packet.thrust, 0.5f)) {
+    //                 climb_rate_or_thrust = 0.0f;
+    //             } else if (packet.thrust > 0.5f) {
+    //                 // climb at up to WPNAV_SPEED_UP
+    //                 climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * blimp.wp_nav->get_default_speed_up();
+    //             } else {
+    //                 // descend at up to WPNAV_SPEED_DN
+    //                 climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -fabsf(blimp.wp_nav->get_default_speed_down());
+    //             }
+    //         }
+
+    //         // if the body_yaw_rate field is ignored, use the commanded yaw position
+    //         // otherwise use the commanded yaw rate
+    //         bool use_yaw_rate = false;
+    //         if ((packet.type_mask & (1<<2)) == 0) {
+    //             use_yaw_rate = true;
+    //         }
+
+    //         blimp.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
+    //                 climb_rate_or_thrust, use_yaw_rate, packet.body_yaw_rate, use_thrust);
+
+    //         break;
+    //     }
+
+    //     case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
+    //     {
+    //         // decode packet
+    //         mavlink_set_position_target_local_ned_t packet;
+    //         mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
+
+    //         // exit if vehicle is not in Guided mode or Auto-Guided mode
+    //         if (!blimp.flightmode->in_guided_mode()) {
+    //             break;
+    //         }
+
+    //         // check for supported coordinate frames
+    //         if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
+    //             packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
+    //             packet.coordinate_frame != MAV_FRAME_BODY_NED &&
+    //             packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
+    //             break;
+    //         }
+
+    //         bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
+    //         bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
+    //         bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
+    //         bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
+    //         bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
+
+    //         // exit immediately if acceleration provided
+    //         if (!acc_ignore) {
+    //             break;
+    //         }
+
+    //         // prepare position
+    //         Vector3f pos_vector;
+    //         if (!pos_ignore) {
+    //             // convert to cm
+    //             pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
+    //             // rotate to body-frame if necessary
+    //             if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
+    //                 packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+    //                 blimp.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y);
+    //             }
+    //             // add body offset if necessary
+    //             if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
+    //                 packet.coordinate_frame == MAV_FRAME_BODY_NED ||
+    //                 packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+    //                 pos_vector += blimp.inertial_nav.get_position();
+    //             }
+    //         }
+
+    //         // prepare velocity
+    //         Vector3f vel_vector;
+    //         if (!vel_ignore) {
+    //             // convert to cm
+    //             vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
+    //             // rotate to body-frame if necessary
+    //             if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+    //                 blimp.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
+    //             }
+    //         }
+
+    //         // prepare yaw
+    //         float yaw_cd = 0.0f;
+    //         bool yaw_relative = false;
+    //         float yaw_rate_cds = 0.0f;
+    //         if (!yaw_ignore) {
+    //             yaw_cd = ToDeg(packet.yaw) * 100.0f;
+    //             yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
+    //         }
+    //         if (!yaw_rate_ignore) {
+    //             yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
+    //         }
+
+    //         // send request
+    //         if (!pos_ignore && !vel_ignore) {
+    //             blimp.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+    //         } else if (pos_ignore && !vel_ignore) {
+    //             blimp.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+    //         } else if (!pos_ignore && vel_ignore) {
+    //             blimp.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+    //         }
+
+    //         break;
+    //     }
+
+    //     case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
+    //     {
+    //         // decode packet
+    //         mavlink_set_position_target_global_int_t packet;
+    //         mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
+
+    //         // exit if vehicle is not in Guided mode or Auto-Guided mode
+    //         if (!blimp.flightmode->in_guided_mode()) {
+    //             break;
+    //         }
+
+    //         bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
+    //         bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
+    //         bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
+    //         bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
+    //         bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
+
+    //         // exit immediately if acceleration provided
+    //         if (!acc_ignore) {
+    //             break;
+    //         }
+
+    //         // extract location from message
+    //         Location loc;
+    //         if (!pos_ignore) {
+    //             // sanity check location
+    //             if (!check_latlng(packet.lat_int, packet.lon_int)) {
+    //                 break;
+    //             }
+    //             Location::AltFrame frame;
+    //             if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
+    //                 // unknown coordinate frame
+    //                 break;
+    //             }
+    //             loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
+    //         }
+
+    //         // prepare yaw
+    //         float yaw_cd = 0.0f;
+    //         bool yaw_relative = false;
+    //         float yaw_rate_cds = 0.0f;
+    //         if (!yaw_ignore) {
+    //             yaw_cd = ToDeg(packet.yaw) * 100.0f;
+    //             yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
+    //         }
+    //         if (!yaw_rate_ignore) {
+    //             yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
+    //         }
+
+    //         // send targets to the appropriate guided mode controller
+    //         if (!pos_ignore && !vel_ignore) {
+    //             // convert Location to vector from ekf origin for posvel controller
+    //             if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
+    //                 // posvel controller does not support alt-above-terrain
+    //                 break;
+    //             }
+    //             Vector3f pos_neu_cm;
+    //             if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
+    //                 break;
+    //             }
+    //             blimp.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+    //         } else if (pos_ignore && !vel_ignore) {
+    //             blimp.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+    //         } else if (!pos_ignore && vel_ignore) {
+    //             blimp.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+    //         }
+
+    //         break;
+    //     }
+    // #endif
+
+    case MAVLINK_MSG_ID_RADIO:
+    case MAVLINK_MSG_ID_RADIO_STATUS: {     // MAV ID: 109
+        handle_radio_status(msg, blimp.should_log(MASK_LOG_PM));
+        break;
+    }
+
+    case MAVLINK_MSG_ID_TERRAIN_DATA:
+    case MAVLINK_MSG_ID_TERRAIN_CHECK:
+        break;
+
+    case MAVLINK_MSG_ID_SET_HOME_POSITION: {
+        mavlink_set_home_position_t packet;
+        mavlink_msg_set_home_position_decode(&msg, &packet);
+        if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
+            if (!blimp.set_home_to_current_location(true)) {
+                // silently ignored
+            }
+        } else {
+            Location new_home_loc;
+            new_home_loc.lat = packet.latitude;
+            new_home_loc.lng = packet.longitude;
+            new_home_loc.alt = packet.altitude / 10;
+            if (!blimp.set_home(new_home_loc, true)) {
+                // silently ignored
+            }
+        }
+        break;
+    }
+
+    case MAVLINK_MSG_ID_ADSB_VEHICLE:
+    case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
+    case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
+    case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
+        break;
+
+    default:
+        handle_common_message(msg);
+        break;
+    }     // end switch
+} // end handle mavlink
+
+
+MAV_RESULT GCS_MAVLINK_Blimp::handle_flight_termination(const mavlink_command_long_t &packet)
+{
+    MAV_RESULT result = MAV_RESULT_FAILED;
+    if (packet.param1 > 0.5f) {
+        blimp.arming.disarm(AP_Arming::Method::TERMINATION);
+        result = MAV_RESULT_ACCEPTED;
+    }
+    return result;
+}
+
+float GCS_MAVLINK_Blimp::vfr_hud_alt() const
+{
+    if (blimp.g2.dev_options.get() & DevOptionVFR_HUDRelativeAlt) {
+        // compatibility option for older mavlink-aware devices that
+        // assume Blimp returns a relative altitude in VFR_HUD.alt
+        return blimp.current_loc.alt * 0.01f;
+    }
+    return GCS_MAVLINK::vfr_hud_alt();
+}
+
+uint64_t GCS_MAVLINK_Blimp::capabilities() const
+{
+    return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
+            MAV_PROTOCOL_CAPABILITY_MISSION_INT |
+            MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
+            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
+            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
+            MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION |
+            MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
+            GCS_MAVLINK::capabilities());
+}
+
+MAV_LANDED_STATE GCS_MAVLINK_Blimp::landed_state() const
+{
+    if (blimp.ap.land_complete) {
+        return MAV_LANDED_STATE_ON_GROUND;
+    }
+    if (blimp.flightmode->is_landing()) {
+        return MAV_LANDED_STATE_LANDING;
+    }
+    // if (blimp.flightmode->is_taking_off()) {
+    //     return MAV_LANDED_STATE_TAKEOFF;
+    // }
+    return MAV_LANDED_STATE_IN_AIR;
+}
+
+void GCS_MAVLINK_Blimp::send_wind() const
+{
+    Vector3f airspeed_vec_bf;
+    if (!AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
+        // if we don't have an airspeed estimate then we don't have a
+        // valid wind estimate on blimps
+        return;
+    }
+    const Vector3f wind = AP::ahrs().wind_estimate();
+    mavlink_msg_wind_send(
+        chan,
+        degrees(atan2f(-wind.y, -wind.x)),
+        wind.length(),
+        wind.z);
+}

Blimp/GCS_Mavlink.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <GCS_MAVLink/GCS.h>
+
+class GCS_MAVLINK_Blimp : public GCS_MAVLINK
+{
+
+public:
+
+    using GCS_MAVLINK::GCS_MAVLINK;
+
+protected:
+
+    uint32_t telem_delay() const override;
+
+    MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override;
+
+    uint8_t sysid_my_gcs() const override;
+    bool sysid_enforce() const override;
+
+    bool params_ready() const override;
+    void send_banner() override;
+
+    MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
+
+    void send_position_target_global_int() override;
+    // void send_position_target_local_ned() override;
+
+    MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;
+    MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet) override;
+    MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override;
+    MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override;
+    MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
+    MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
+
+    // void handle_mount_message(const mavlink_message_t &msg) override;
+
+    bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
+    bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
+    void send_nav_controller_output() const override; //MIR can't comment out or build fails??
+    uint64_t capabilities() const override;
+
+    virtual MAV_VTOL_STATE vtol_state() const override
+    {
+        return MAV_VTOL_STATE_MC;
+    };
+    virtual MAV_LANDED_STATE landed_state() const override;
+
+private:
+
+    void handleMessage(const mavlink_message_t &msg) override;
+    void handle_command_ack(const mavlink_message_t &msg) override;
+    bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
+    void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
+    void handle_rc_channels_override(const mavlink_message_t &msg) override;
+    bool try_send_message(enum ap_message id) override;
+
+    void packetReceived(const mavlink_status_t &status,
+                        const mavlink_message_t &msg) override;
+
+    MAV_MODE base_mode() const override;
+    MAV_STATE vehicle_system_status() const override;
+
+    float vfr_hud_airspeed() const override;
+    int16_t vfr_hud_throttle() const override;
+    float vfr_hud_alt() const override;
+
+    void send_pid_tuning() override;
+
+    void send_wind() const;
+};

Blimp/Log.cpp

@@ -0,0 +1,555 @@
+#include "Blimp.h"
+
+#if LOGGING_ENABLED == ENABLED
+
+// Code to Write and Read packets from AP_Logger log memory
+// Code to interact with the user to dump or erase logs
+
+struct PACKED log_Control_Tuning {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float    throttle_in;
+    float    angle_boost;
+    float    throttle_out;
+    float    throttle_hover;
+    float    desired_alt;
+    float    inav_alt;
+    int32_t  baro_alt;
+    float    desired_rangefinder_alt;
+    float    rangefinder_alt;
+    float    terr_alt;
+    int16_t  target_climb_rate;
+    int16_t  climb_rate;
+};
+
+// Write a control tuning packet
+void Blimp::Log_Write_Control_Tuning()
+{
+    // get terrain altitude
+    // float terr_alt = 0.0f;
+    // float des_alt_m = 0.0f;
+    // int16_t target_climb_rate_cms = 0;
+    // if (!flightmode->has_manual_throttle()) {
+    //     des_alt_m = pos_control->get_alt_target() / 100.0f;
+    //     target_climb_rate_cms = pos_control->get_vel_target_z();
+    // }
+
+    // get surface tracking alts
+    // float desired_rangefinder_alt;
+    // if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) {
+    // desired_rangefinder_alt = AP::logger().quiet_nan();
+    // }
+
+    // struct log_Control_Tuning pkt = {
+    //     LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
+    //     time_us             : AP_HAL::micros64(),
+    //     throttle_in         : attitude_control->get_throttle_in(),
+    //     angle_boost         : attitude_control->angle_boost(),
+    //     throttle_out        : motors->get_throttle(),
+    //     throttle_hover      : motors->get_throttle_hover(),
+    //     desired_alt         : des_alt_m,
+    //     inav_alt            : inertial_nav.get_altitude() / 100.0f,
+    //     baro_alt            : baro_alt,
+    //     desired_rangefinder_alt : desired_rangefinder_alt,
+    //     rangefinder_alt     : surface_tracking.get_dist_for_logging(),
+    //     terr_alt            : terr_alt,
+    //     target_climb_rate   : target_climb_rate_cms,
+    //     climb_rate          : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
+    // };
+    // logger.WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write an attitude packet
+void Blimp::Log_Write_Attitude()
+{
+    // // Vector3f targets = attitude_control->get_att_target_euler_cd();
+    // targets.z = wrap_360_cd(targets.z);
+    // logger.Write_Attitude(targets);
+    // logger.Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
+    // if (should_log(MASK_LOG_PID)) {
+    //     logger.Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
+    //     logger.Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info());
+    //     logger.Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info());
+    //     // logger.Write_PID(LOG_PIDA_MSG, pos_control->get_accel_z_pid().get_pid_info() );
+    // }
+}
+
+// Write an EKF and POS packet
+void Blimp::Log_Write_EKF_POS()
+{
+    AP::ahrs_navekf().Log_Write();
+    ahrs.Write_AHRS2();
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    sitl.Log_Write_SIMSTATE();
+#endif
+    ahrs.Write_POS();
+}
+
+struct PACKED log_MotBatt {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float   lift_max;
+    float   bat_volt;
+    float   bat_res;
+    float   th_limit;
+};
+
+// Write an rate packet
+void Blimp::Log_Write_MotBatt()
+{
+    // struct log_MotBatt pkt_mot = {
+    //     LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
+    //     time_us         : AP_HAL::micros64(),
+    //     lift_max        : (float)(motors->get_lift_max()),
+    //     bat_volt        : (float)(motors->get_batt_voltage_filt()),
+    //     bat_res         : (float)(battery.get_resistance()),
+    //     th_limit        : (float)(motors->get_throttle_limit())
+    // };
+    // logger.WriteBlock(&pkt_mot, sizeof(pkt_mot));
+}
+
+struct PACKED log_Data_Int16t {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t id;
+    int16_t data_value;
+};
+
+// Write an int16_t data packet
+UNUSED_FUNCTION
+void Blimp::Log_Write_Data(LogDataID id, int16_t value)
+{
+    if (should_log(MASK_LOG_ANY)) {
+        struct log_Data_Int16t pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG),
+time_us     : AP_HAL::micros64(),
+id          : (uint8_t)id,
+data_value  : value
+        };
+        logger.WriteCriticalBlock(&pkt, sizeof(pkt));
+    }
+}
+
+struct PACKED log_Data_UInt16t {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t id;
+    uint16_t data_value;
+};
+
+// Write an uint16_t data packet
+UNUSED_FUNCTION
+void Blimp::Log_Write_Data(LogDataID id, uint16_t value)
+{
+    if (should_log(MASK_LOG_ANY)) {
+        struct log_Data_UInt16t pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG),
+time_us     : AP_HAL::micros64(),
+id          : (uint8_t)id,
+data_value  : value
+        };
+        logger.WriteCriticalBlock(&pkt, sizeof(pkt));
+    }
+}
+
+struct PACKED log_Data_Int32t {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t id;
+    int32_t data_value;
+};
+
+// Write an int32_t data packet
+void Blimp::Log_Write_Data(LogDataID id, int32_t value)
+{
+    if (should_log(MASK_LOG_ANY)) {
+        struct log_Data_Int32t pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG),
+time_us  : AP_HAL::micros64(),
+id          : (uint8_t)id,
+data_value  : value
+        };
+        logger.WriteCriticalBlock(&pkt, sizeof(pkt));
+    }
+}
+
+struct PACKED log_Data_UInt32t {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t id;
+    uint32_t data_value;
+};
+
+// Write a uint32_t data packet
+void Blimp::Log_Write_Data(LogDataID id, uint32_t value)
+{
+    if (should_log(MASK_LOG_ANY)) {
+        struct log_Data_UInt32t pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG),
+time_us     : AP_HAL::micros64(),
+id          : (uint8_t)id,
+data_value  : value
+        };
+        logger.WriteCriticalBlock(&pkt, sizeof(pkt));
+    }
+}
+
+struct PACKED log_Data_Float {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t id;
+    float data_value;
+};
+
+// Write a float data packet
+UNUSED_FUNCTION
+void Blimp::Log_Write_Data(LogDataID id, float value)
+{
+    if (should_log(MASK_LOG_ANY)) {
+        struct log_Data_Float pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG),
+time_us     : AP_HAL::micros64(),
+id          : (uint8_t)id,
+data_value  : value
+        };
+        logger.WriteCriticalBlock(&pkt, sizeof(pkt));
+    }
+}
+
+struct PACKED log_ParameterTuning {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t  parameter;     // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE
+    float    tuning_value;  // normalized value used inside tuning() function
+    float    tuning_min;    // tuning minimum value
+    float    tuning_max;    // tuning maximum value
+};
+
+void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max)
+{
+    struct log_ParameterTuning pkt_tune = {
+        LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG),
+time_us        : AP_HAL::micros64(),
+parameter      : param,
+tuning_value   : tuning_val,
+tuning_min     : tune_min,
+tuning_max     : tune_max
+    };
+
+    logger.WriteBlock(&pkt_tune, sizeof(pkt_tune));
+}
+
+// logs when baro or compass becomes unhealthy
+void Blimp::Log_Sensor_Health()
+{
+    // check baro
+    if (sensor_health.baro != barometer.healthy()) {
+        sensor_health.baro = barometer.healthy();
+        AP::logger().Write_Error(LogErrorSubsystem::BARO,
+                                 (sensor_health.baro ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY));
+    }
+
+    // check compass
+    if (sensor_health.compass != compass.healthy()) {
+        sensor_health.compass = compass.healthy();
+        AP::logger().Write_Error(LogErrorSubsystem::COMPASS, (sensor_health.compass ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY));
+    }
+
+    // check primary GPS
+    if (sensor_health.primary_gps != gps.primary_sensor()) {
+        sensor_health.primary_gps = gps.primary_sensor();
+        AP::logger().Write_Event(LogEvent::GPS_PRIMARY_CHANGED);
+    }
+}
+
+struct PACKED log_SysIdD {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float    waveform_time;
+    float    waveform_sample;
+    float    waveform_freq;
+    float    angle_x;
+    float    angle_y;
+    float    angle_z;
+    float    accel_x;
+    float    accel_y;
+    float    accel_z;
+};
+
+// Write an rate packet
+void Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z)
+{
+#if MODE_SYSTEMID_ENABLED == ENABLED
+    struct log_SysIdD pkt_sidd = {
+        LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG),
+time_us         : AP_HAL::micros64(),
+waveform_time   : waveform_time,
+waveform_sample : waveform_sample,
+waveform_freq   : waveform_freq,
+angle_x         : angle_x,
+angle_y         : angle_y,
+angle_z         : angle_z,
+accel_x         : accel_x,
+accel_y         : accel_y,
+accel_z         : accel_z
+    };
+    logger.WriteBlock(&pkt_sidd, sizeof(pkt_sidd));
+#endif
+}
+
+struct PACKED log_SysIdS {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t  systemID_axis;
+    float    waveform_magnitude;
+    float    frequency_start;
+    float    frequency_stop;
+    float    time_fade_in;
+    float    time_const_freq;
+    float    time_record;
+    float    time_fade_out;
+};
+
+// Write an rate packet
+void Blimp::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out)
+{
+#if MODE_SYSTEMID_ENABLED == ENABLED
+    struct log_SysIdS pkt_sids = {
+        LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG),
+time_us             : AP_HAL::micros64(),
+systemID_axis       : systemID_axis,
+waveform_magnitude  : waveform_magnitude,
+frequency_start     : frequency_start,
+frequency_stop      : frequency_stop,
+time_fade_in        : time_fade_in,
+time_const_freq     : time_const_freq,
+time_record         : time_record,
+time_fade_out       : time_fade_out
+    };
+    logger.WriteBlock(&pkt_sids, sizeof(pkt_sids));
+#endif
+}
+
+// // guided target logging
+// struct PACKED log_GuidedTarget {
+//     LOG_PACKET_HEADER;
+//     uint64_t time_us;
+//     uint8_t type;
+//     float pos_target_x;
+//     float pos_target_y;
+//     float pos_target_z;
+//     float vel_target_x;
+//     float vel_target_y;
+//     float vel_target_z;
+// };
+
+// Write a Guided mode target
+// pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees
+// vel_target is cm/s
+// void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
+// {
+//     struct log_GuidedTarget pkt = {
+//         LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
+//         time_us         : AP_HAL::micros64(),
+//         type            : target_type,
+//         pos_target_x    : pos_target.x,
+//         pos_target_y    : pos_target.y,
+//         pos_target_z    : pos_target.z,
+//         vel_target_x    : vel_target.x,
+//         vel_target_y    : vel_target.y,
+//         vel_target_z    : vel_target.z
+//     };
+//     logger.WriteBlock(&pkt, sizeof(pkt));
+// }
+
+// type and unit information can be found in
+// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
+// units and "Format characters" for field type information
+const struct LogStructure Blimp::log_structure[] = {
+    LOG_COMMON_STRUCTURES,
+
+    // @LoggerMessage: PTUN
+    // @Description: Parameter Tuning information
+    // @URL: https://ardupilot.org/blimp/docs/tuning.html#in-flight-tuning
+    // @Field: TimeUS: Time since system startup
+    // @Field: Param: Parameter being tuned
+    // @Field: TunVal: Normalized value used inside tuning() function
+    // @Field: TunMin: Tuning minimum limit
+    // @Field: TunMax: Tuning maximum limit
+
+    {
+        LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
+        "PTUN", "QBfff",         "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----"
+    },
+
+    // @LoggerMessage: CTUN
+    // @Description: Control Tuning information
+    // @Field: TimeUS: Time since system startup
+    // @Field: ThI: throttle input
+    // @Field: ABst: angle boost
+    // @Field: ThO: throttle output
+    // @Field: ThH: calculated hover throttle
+    // @Field: DAlt: desired altitude
+    // @Field: Alt: achieved altitude
+    // @Field: BAlt: barometric altitude
+    // @Field: DSAlt: desired rangefinder altitude
+    // @Field: SAlt: achieved rangefinder altitude
+    // @Field: TAlt: terrain altitude
+    // @Field: DCRt: desired climb rate
+    // @Field: CRt: climb rate
+
+    // @LoggerMessage: D16
+    // @Description: Generic 16-bit-signed-integer storage
+    // @Field: TimeUS: Time since system startup
+    // @Field: Id: Data type identifier
+    // @Field: Value: Value
+
+    // @LoggerMessage: DU16
+    // @Description: Generic 16-bit-unsigned-integer storage
+    // @Field: TimeUS: Time since system startup
+    // @Field: Id: Data type identifier
+    // @Field: Value: Value
+
+    // @LoggerMessage: D32
+    // @Description: Generic 32-bit-signed-integer storage
+    // @Field: TimeUS: Time since system startup
+    // @Field: Id: Data type identifier
+    // @Field: Value: Value
+
+    // @LoggerMessage: DFLT
+    // @Description: Generic float storage
+    // @Field: TimeUS: Time since system startup
+    // @Field: Id: Data type identifier
+    // @Field: Value: Value
+
+    // @LoggerMessage: DU32
+    // @Description: Generic 32-bit-unsigned-integer storage
+    // @Field: TimeUS: Time since system startup
+    // @Field: Id: Data type identifier
+    // @Field: Value: Value
+
+    {
+        LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
+        "CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B000BB"
+    },
+
+    // @LoggerMessage: MOTB
+    // @Description: Battery information
+    // @Field: TimeUS: Time since system startup
+    // @Field: LiftMax: Maximum motor compensation gain
+    // @Field: BatVolt: Ratio betwen detected battery voltage and maximum battery voltage
+    // @Field: BatRes: Estimated battery resistance
+    // @Field: ThLimit: Throttle limit set due to battery current limitations
+
+    {
+        LOG_MOTBATT_MSG, sizeof(log_MotBatt),
+        "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-"
+    },
+    {
+        LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),
+        "D16",   "QBh",         "TimeUS,Id,Value", "s--", "F--"
+    },
+    {
+        LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),
+        "DU16",  "QBH",         "TimeUS,Id,Value", "s--", "F--"
+    },
+    {
+        LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),
+        "D32",   "QBi",         "TimeUS,Id,Value", "s--", "F--"
+    },
+    {
+        LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),
+        "DU32",  "QBI",         "TimeUS,Id,Value", "s--", "F--"
+    },
+    {
+        LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),
+        "DFLT",  "QBf",         "TimeUS,Id,Value", "s--", "F--"
+    },
+
+    // @LoggerMessage: SIDD
+    // @Description: System ID data
+    // @Field: TimeUS: Time since system startup
+    // @Field: Time: Time reference for waveform
+    // @Field: Targ: Current waveform sample
+    // @Field: F: Instantaneous waveform frequency
+    // @Field: Gx: Delta angle, X-Axis
+    // @Field: Gy: Delta angle, Y-Axis
+    // @Field: Gz: Delta angle, Z-Axis
+    // @Field: Ax: Delta velocity, X-Axis
+    // @Field: Ay: Delta velocity, Y-Axis
+    // @Field: Az: Delta velocity, Z-Axis
+
+    {
+        LOG_SYSIDD_MSG, sizeof(log_SysIdD),
+        "SIDD", "Qfffffffff",  "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az", "ss-zkkkooo", "F---------"
+    },
+
+    // @LoggerMessage: SIDS
+    // @Description: System ID settings
+    // @Field: TimeUS: Time since system startup
+    // @Field: Ax: The axis which is being excited
+    // @Field: Mag: Magnitude of the chirp waveform
+    // @Field: FSt: Frequency at the start of chirp
+    // @Field: FSp: Frequency at the end of chirp
+    // @Field: TFin: Time to reach maximum amplitude of chirp
+    // @Field: TC: Time at constant frequency before chirp starts
+    // @Field: TR: Time taken to complete chirp waveform
+    // @Field: TFout: Time to reach zero amplitude after chirp finishes
+
+    {
+        LOG_SYSIDS_MSG, sizeof(log_SysIdS),
+        "SIDS", "QBfffffff",  "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------"
+    },
+
+    // // @LoggerMessage: GUID
+    // // @Description: Guided mode target information
+    // // @Field: TimeUS: Time since system startup
+    // // @Field: Type: Type of guided mode
+    // // @Field: pX: Target position, X-Axis
+    // // @Field: pY: Target position, Y-Axis
+    // // @Field: pZ: Target position, Z-Axis
+    // // @Field: vX: Target velocity, X-Axis
+    // // @Field: vY: Target velocity, Y-Axis
+    // // @Field: vZ: Target velocity, Z-Axis
+
+    //     { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
+    //       "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-BBBBBB" },
+};
+
+void Blimp::Log_Write_Vehicle_Startup_Messages()
+{
+    // only 200(?) bytes are guaranteed by AP_Logger
+    logger.Write_MessageF("Frame: %s", get_frame_string());
+    logger.Write_Mode((uint8_t)control_mode, control_mode_reason);
+    ahrs.Log_Write_Home_And_Origin();
+    gps.Write_AP_Logger_Log_Startup_messages();
+}
+
+void Blimp::log_init(void)
+{
+    logger.Init(log_structure, ARRAY_SIZE(log_structure));
+}
+
+#else // LOGGING_ENABLED
+
+void Blimp::Log_Write_Control_Tuning() {}
+void Blimp::Log_Write_Performance() {}
+void Blimp::Log_Write_Attitude(void) {}
+void Blimp::Log_Write_EKF_POS() {}
+void Blimp::Log_Write_MotBatt() {}
+void Blimp::Log_Write_Data(LogDataID id, int32_t value) {}
+void Blimp::Log_Write_Data(LogDataID id, uint32_t value) {}
+void Blimp::Log_Write_Data(LogDataID id, int16_t value) {}
+void Blimp::Log_Write_Data(LogDataID id, uint16_t value) {}
+void Blimp::Log_Write_Data(LogDataID id, float value) {}
+void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
+void Blimp::Log_Sensor_Health() {}
+void Blimp::Log_Write_Precland() {}
+void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
+void Blimp::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {}
+void Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {}
+void Blimp::Log_Write_Vehicle_Startup_Messages() {}
+
+void Blimp::log_init(void) {}
+
+#endif // LOGGING_ENABLED

Blimp/Parameters.cpp

@@ -0,0 +1,727 @@
+#include "Blimp.h"
+
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *  Blimp parameter definitions
+ *
+ */
+
+#define GSCALAR(v, name, def) { blimp.g.v.vtype, name, Parameters::k_param_ ## v, &blimp.g.v, {def_value : def} }
+#define ASCALAR(v, name, def) { blimp.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&blimp.aparm.v, {def_value : def} }
+#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &blimp.g.v, {group_info : class::var_info} }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&blimp.v, {group_info : class::var_info} }
+#define GOBJECTPTR(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&blimp.v, {group_info : class::var_info}, AP_PARAM_FLAG_POINTER }
+#define GOBJECTVARPTR(v, name, var_info_ptr) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&blimp.v, {group_info_ptr : var_info_ptr}, AP_PARAM_FLAG_POINTER | AP_PARAM_FLAG_INFO_POINTER }
+#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&blimp.v, {group_info : class::var_info} }
+
+#define DEFAULT_FRAME_CLASS 0
+
+const AP_Param::Info Blimp::var_info[] = {
+    // @Param: FORMAT_VERSION
+    // @DisplayName: Eeprom format version number
+    // @Description: This value is incremented when changes are made to the eeprom format
+    // @User: Advanced
+    // @ReadOnly: True
+    GSCALAR(format_version, "FORMAT_VERSION",   0),
+
+    // @Param: SYSID_THISMAV
+    // @DisplayName: MAVLink system ID of this vehicle
+    // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
+    // @Range: 1 255
+    // @User: Advanced
+    GSCALAR(sysid_this_mav, "SYSID_THISMAV",   MAV_SYSTEM_ID),
+
+    // @Param: SYSID_MYGCS
+    // @DisplayName: My ground station number
+    // @Description: Allows restricting radio overrides to only come from my ground station
+    // @Range: 1 255
+    // @User: Advanced
+    GSCALAR(sysid_my_gcs,   "SYSID_MYGCS",     255),
+
+    // @Param: PILOT_THR_FILT
+    // @DisplayName: Throttle filter cutoff
+    // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable
+    // @User: Advanced
+    // @Units: Hz
+    // @Range: 0 10
+    // @Increment: .5
+    GSCALAR(throttle_filt,  "PILOT_THR_FILT",     0),
+
+    // @Param: PILOT_TKOFF_ALT
+    // @DisplayName: Pilot takeoff altitude
+    // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick.
+    // @User: Standard
+    // @Units: cm
+    // @Range: 0.0 1000.0
+    // @Increment: 10
+    GSCALAR(pilot_takeoff_alt,  "PILOT_TKOFF_ALT",  PILOT_TKOFF_ALT_DEFAULT),
+
+    // @Param: PILOT_THR_BHV
+    // @DisplayName: Throttle stick behavior
+    // @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick.
+    // @User: Standard
+    // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing,4:Disarm on land detection
+    // @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection
+    GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0),
+
+    // @Group: SERIAL
+    // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
+    GOBJECT(serial_manager, "SERIAL",   AP_SerialManager),
+
+    // @Param: TELEM_DELAY
+    // @DisplayName: Telemetry startup delay
+    // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up
+    // @User: Advanced
+    // @Units: s
+    // @Range: 0 30
+    // @Increment: 1
+    GSCALAR(telem_delay,            "TELEM_DELAY",     0),
+
+    // @Param: GCS_PID_MASK
+    // @DisplayName: GCS PID tuning mask
+    // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
+    // @User: Advanced
+    // @Values: 0:None,1:Roll,2:Pitch,4:Yaw,8:AccelZ
+    // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ
+    GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
+
+    // #if MODE_RTL_ENABLED == ENABLED
+    //     // @Param: RTL_ALT
+    //     // @DisplayName: RTL Altitude
+    //     // @Description: The minimum alt above home the vehicle will climb to before returning.  If the vehicle is flying higher than this value it will return at its current altitude.
+    //     // @Units: cm
+    //     // @Range: 200 8000
+    //     // @Increment: 1
+    //     // @User: Standard
+    //     GSCALAR(rtl_altitude,   "RTL_ALT",     RTL_ALT),
+
+    //     // @Param: RTL_CONE_SLOPE
+    //     // @DisplayName: RTL cone slope
+    //     // @Description: Defines a cone above home which determines maximum climb
+    //     // @Range: 0.5 10.0
+    //     // @Increment: .1
+    //     // @Values: 0:Disabled,1:Shallow,3:Steep
+    //     // @User: Standard
+    //     GSCALAR(rtl_cone_slope,   "RTL_CONE_SLOPE",     RTL_CONE_SLOPE_DEFAULT),
+
+    //     // @Param: RTL_SPEED
+    //     // @DisplayName: RTL speed
+    //     // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead.
+    //     // @Units: cm/s
+    //     // @Range: 0 2000
+    //     // @Increment: 50
+    //     // @User: Standard
+    //     GSCALAR(rtl_speed_cms,   "RTL_SPEED",     0),
+
+    //     // @Param: RTL_ALT_FINAL
+    //     // @DisplayName: RTL Final Altitude
+    //     // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission.  Set to zero to land.
+    //     // @Units: cm
+    //     // @Range: 0 1000
+    //     // @Increment: 1
+    //     // @User: Standard
+    //     GSCALAR(rtl_alt_final,  "RTL_ALT_FINAL", RTL_ALT_FINAL),
+
+    //     // @Param: RTL_CLIMB_MIN
+    //     // @DisplayName: RTL minimum climb
+    //     // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL
+    //     // @Units: cm
+    //     // @Range: 0 3000
+    //     // @Increment: 10
+    //     // @User: Standard
+    //     GSCALAR(rtl_climb_min,  "RTL_CLIMB_MIN",    RTL_CLIMB_MIN_DEFAULT),
+
+    //     // @Param: RTL_LOIT_TIME
+    //     // @DisplayName: RTL loiter time
+    //     // @Description: Time (in milliseconds) to loiter above home before beginning final descent
+    //     // @Units: ms
+    //     // @Range: 0 60000
+    //     // @Increment: 1000
+    //     // @User: Standard
+    //     GSCALAR(rtl_loiter_time,      "RTL_LOIT_TIME",    RTL_LOITER_TIME),
+    // #endif
+
+    // @Param: FS_GCS_ENABLE
+    // @DisplayName: Ground Station Failsafe Enable
+    // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled.
+    // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land (4.0+ Only)
+    // @User: Standard
+    GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),
+
+    // @Param: GPS_HDOP_GOOD
+    // @DisplayName: GPS Hdop Good
+    // @Description: GPS Hdop value at or below this value represent a good position.  Used for pre-arm checks
+    // @Range: 100 900
+    // @User: Advanced
+    GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT),
+
+    // @Param: WP_YAW_BEHAVIOR
+    // @DisplayName: Yaw behaviour during missions
+    // @Description: Determines how the autopilot controls the yaw during missions and RTL
+    // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course
+    // @User: Standard
+    GSCALAR(wp_yaw_behavior,  "WP_YAW_BEHAVIOR",    WP_YAW_BEHAVIOR_DEFAULT),
+
+    // @Param: LAND_SPEED
+    // @DisplayName: Land speed
+    // @Description: The descent speed for the final stage of landing in cm/s
+    // @Units: cm/s
+    // @Range: 30 200
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(land_speed,             "LAND_SPEED",   LAND_SPEED),
+
+    // @Param: LAND_SPEED_HIGH
+    // @DisplayName: Land speed high
+    // @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used
+    // @Units: cm/s
+    // @Range: 0 500
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(land_speed_high,        "LAND_SPEED_HIGH",   0),
+
+    // @Param: PILOT_SPEED_UP
+    // @DisplayName: Pilot maximum vertical speed ascending
+    // @Description: The maximum vertical ascending velocity the pilot may request in cm/s
+    // @Units: cm/s
+    // @Range: 50 500
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(pilot_speed_up,     "PILOT_SPEED_UP",   PILOT_VELZ_MAX),
+
+    // @Param: PILOT_ACCEL_Z
+    // @DisplayName: Pilot vertical acceleration
+    // @Description: The vertical acceleration used when pilot is controlling the altitude
+    // @Units: cm/s/s
+    // @Range: 50 500
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(pilot_accel_z,  "PILOT_ACCEL_Z",    PILOT_ACCEL_Z_DEFAULT),
+
+    // @Param: FS_THR_ENABLE
+    // @DisplayName: Throttle Failsafe Enable
+    // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
+    // @Values:  0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land
+    // @User: Standard
+    GSCALAR(failsafe_throttle,  "FS_THR_ENABLE",   FS_THR_ENABLED_ALWAYS_RTL),
+
+    // @Param: FS_THR_VALUE
+    // @DisplayName: Throttle Failsafe Value
+    // @Description: The PWM level in microseconds on channel 3 below which throttle failsafe triggers
+    // @Range: 910 1100
+    // @Units: PWM
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(failsafe_throttle_value, "FS_THR_VALUE",      FS_THR_VALUE_DEFAULT),
+
+    // @Param: THR_DZ
+    // @DisplayName: Throttle deadzone
+    // @Description: The deadzone above and below mid throttle in PWM microseconds. Used in AltHold, Loiter, PosHold flight modes
+    // @User: Standard
+    // @Range: 0 300
+    // @Units: PWM
+    // @Increment: 1
+    GSCALAR(throttle_deadzone,  "THR_DZ",    THR_DZ_DEFAULT),
+
+    // @Param: FLTMODE1
+    // @DisplayName: Flight Mode 1
+    // @Description: Flight mode when Channel 5 pwm is <= 1230
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
+    // @User: Standard
+    GSCALAR(flight_mode1, "FLTMODE1",               (uint8_t)FLIGHT_MODE_1),
+
+    // @Param: FLTMODE2
+    // @DisplayName: Flight Mode 2
+    // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
+    // @User: Standard
+    GSCALAR(flight_mode2, "FLTMODE2",               (uint8_t)FLIGHT_MODE_2),
+
+    // @Param: FLTMODE3
+    // @DisplayName: Flight Mode 3
+    // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
+    // @User: Standard
+    GSCALAR(flight_mode3, "FLTMODE3",               (uint8_t)FLIGHT_MODE_3),
+
+    // @Param: FLTMODE4
+    // @DisplayName: Flight Mode 4
+    // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
+    // @User: Standard
+    GSCALAR(flight_mode4, "FLTMODE4",               (uint8_t)FLIGHT_MODE_4),
+
+    // @Param: FLTMODE5
+    // @DisplayName: Flight Mode 5
+    // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
+    // @User: Standard
+    GSCALAR(flight_mode5, "FLTMODE5",               (uint8_t)FLIGHT_MODE_5),
+
+    // @Param: FLTMODE6
+    // @DisplayName: Flight Mode 6
+    // @Description: Flight mode when Channel 5 pwm is >=1750
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
+    // @User: Standard
+    GSCALAR(flight_mode6, "FLTMODE6",               (uint8_t)FLIGHT_MODE_6),
+
+    // @Param: FLTMODE_CH
+    // @DisplayName: Flightmode channel
+    // @Description: RC Channel to use for flight mode control
+    // @Values: 0:Disabled,5:Channel5,6:Channel6,7:Channel7,8:Channel8
+    // @User: Advanced
+    GSCALAR(flight_mode_chan, "FLTMODE_CH",         CH_MODE_DEFAULT),
+
+    // @Param: INITIAL_MODE
+    // @DisplayName: Initial flight mode
+    // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver.
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
+    // @User: Advanced
+    GSCALAR(initial_mode,        "INITIAL_MODE",     (uint8_t)Mode::Number::MANUAL),
+
+    // @Param: LOG_BITMASK
+    // @DisplayName: Log bitmask
+    // @Description: 4 byte bitmap of log types to enable
+    // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled
+    // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,17:MOTBATT,18:IMU_FAST,19:IMU_RAW
+    // @User: Standard
+    GSCALAR(log_bitmask,    "LOG_BITMASK",          DEFAULT_LOG_BITMASK),
+
+    // @Param: TUNE
+    // @DisplayName: Channel 6 Tuning
+    // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob
+    // @User: Standard
+    // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,58:SysID Magnitude
+    GSCALAR(radio_tuning, "TUNE",                   0),
+
+    // @Param: FRAME_TYPE
+    // @DisplayName: Frame Type (+, X, V, etc)
+    // @Description: Controls motor mixing for multiblimps.  Not used for Tri or Traditional Heliblimps.
+    // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I, 18: BetaFlightXReversed
+    // @User: Standard
+    // @RebootRequired: True
+    GSCALAR(frame_type, "FRAME_TYPE", HAL_FRAME_TYPE_DEFAULT),
+
+    // @Group: ARMING_
+    // @Path: ../libraries/AP_Arming/AP_Arming.cpp
+    GOBJECT(arming,                 "ARMING_", AP_Arming_Blimp),
+
+    // @Param: DISARM_DELAY
+    // @DisplayName: Disarm delay
+    // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm.
+    // @Units: s
+    // @Range: 0 127
+    // @User: Advanced
+    GSCALAR(disarm_delay, "DISARM_DELAY",           AUTO_DISARMING_DELAY),
+
+    // @Param: ANGLE_MAX
+    // @DisplayName: Angle Max
+    // @Description: Maximum lean angle in all flight modes
+    // @Units: cdeg
+    // @Range: 1000 8000
+    // @User: Advanced
+    ASCALAR(angle_max, "ANGLE_MAX",                 DEFAULT_ANGLE_MAX),
+
+    // @Param: PHLD_BRAKE_RATE
+    // @DisplayName: PosHold braking rate
+    // @Description: PosHold flight mode's rotation rate during braking in deg/sec
+    // @Units: deg/s
+    // @Range: 4 12
+    // @User: Advanced
+    GSCALAR(poshold_brake_rate, "PHLD_BRAKE_RATE",  POSHOLD_BRAKE_RATE_DEFAULT),
+
+    // @Param: PHLD_BRAKE_ANGLE
+    // @DisplayName: PosHold braking angle max
+    // @Description: PosHold flight mode's max lean angle during braking in centi-degrees
+    // @Units: cdeg
+    // @Range: 2000 4500
+    // @User: Advanced
+    GSCALAR(poshold_brake_angle_max, "PHLD_BRAKE_ANGLE",  POSHOLD_BRAKE_ANGLE_DEFAULT),
+
+    // @Param: LAND_REPOSITION
+    // @DisplayName: Land repositioning
+    // @Description: Enables user input during LAND mode, the landing phase of RTL, and auto mode landings.
+    // @Values: 0:No repositioning, 1:Repositioning
+    // @User: Advanced
+    GSCALAR(land_repositioning, "LAND_REPOSITION",     LAND_REPOSITION_DEFAULT),
+
+    // @Param: FS_EKF_ACTION
+    // @DisplayName: EKF Failsafe Action
+    // @Description: Controls the action that will be taken when an EKF failsafe is invoked
+    // @Values: 1:Land, 2:AltHold, 3:Land even in Stabilize
+    // @User: Advanced
+    GSCALAR(fs_ekf_action, "FS_EKF_ACTION",    FS_EKF_ACTION_DEFAULT),
+
+    // @Param: FS_EKF_THRESH
+    // @DisplayName: EKF failsafe variance threshold
+    // @Description: Allows setting the maximum acceptable compass and velocity variance
+    // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed
+    // @User: Advanced
+    GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH",    FS_EKF_THRESHOLD_DEFAULT),
+
+    // @Param: FS_CRASH_CHECK
+    // @DisplayName: Crash check enable
+    // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected.
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Advanced
+    GSCALAR(fs_crash_check, "FS_CRASH_CHECK",    1),
+
+    // @Param: RC_SPEED
+    // @DisplayName: ESC Update Speed
+    // @Description: This is the speed in Hertz that your ESCs will receive updates
+    // @Units: Hz
+    // @Range: 50 490
+    // @Increment: 1
+    // @User: Advanced
+    GSCALAR(rc_speed, "RC_SPEED",              RC_FAST_SPEED),
+
+    // @Param: ACRO_RP_P
+    // @DisplayName: Acro Roll and Pitch P gain
+    // @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode.  Higher values mean faster rate of rotation.
+    // @Range: 1 10
+    // @User: Standard
+    GSCALAR(acro_rp_p,                 "ACRO_RP_P",           ACRO_RP_P),
+
+    // @Param: ACRO_YAW_P
+    // @DisplayName: Acro Yaw P gain
+    // @Description: Converts pilot yaw input into a desired rate of rotation.  Higher values mean faster rate of rotation.
+    // @Range: 1 10
+    // @User: Standard
+    GSCALAR(acro_yaw_p,                 "ACRO_YAW_P",           ACRO_YAW_P),
+
+    // variables not in the g class which contain EEPROM saved variables
+
+    // @Group: RELAY_
+    // @Path: ../libraries/AP_Relay/AP_Relay.cpp
+    GOBJECT(relay,                  "RELAY_", AP_Relay),
+
+    // @Group: COMPASS_
+    // @Path: ../libraries/AP_Compass/AP_Compass.cpp
+    GOBJECT(compass,        "COMPASS_", Compass),
+
+    // @Group: INS_
+    // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
+    GOBJECT(ins,            "INS_", AP_InertialSensor),
+
+    // // @Group: WPNAV_
+    // // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
+    // GOBJECTPTR(wp_nav, "WPNAV_",       AC_WPNav),
+
+    // // @Group: LOIT_
+    // // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
+    // GOBJECTPTR(loiter_nav, "LOIT_", AC_Loiter),
+
+    // @Group: ATC_
+    // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
+    // #if FRAME_CONFIG == HELI_FRAME
+    //     GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli),
+    // #else
+    //     GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi),
+    // #endif
+
+    // @Group: PSC
+    // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
+    // GOBJECTPTR(pos_control, "PSC", AC_PosControl),
+
+    // @Group: SR0_
+    // @Path: GCS_Mavlink.cpp
+    GOBJECTN(_gcs.chan_parameters[0],  gcs0,       "SR0_",     GCS_MAVLINK_Parameters),
+
+#if MAVLINK_COMM_NUM_BUFFERS >= 2
+    // @Group: SR1_
+    // @Path: GCS_Mavlink.cpp
+    GOBJECTN(_gcs.chan_parameters[1],  gcs1,       "SR1_",     GCS_MAVLINK_Parameters),
+#endif
+
+#if MAVLINK_COMM_NUM_BUFFERS >= 3
+    // @Group: SR2_
+    // @Path: GCS_Mavlink.cpp
+    GOBJECTN(_gcs.chan_parameters[2],  gcs2,       "SR2_",     GCS_MAVLINK_Parameters),
+#endif
+
+#if MAVLINK_COMM_NUM_BUFFERS >= 4
+    // @Group: SR3_
+    // @Path: GCS_Mavlink.cpp
+    GOBJECTN(_gcs.chan_parameters[3],  gcs3,       "SR3_",     GCS_MAVLINK_Parameters),
+#endif
+
+#if MAVLINK_COMM_NUM_BUFFERS >= 5
+    // @Group: SR4_
+    // @Path: GCS_Mavlink.cpp
+    GOBJECTN(_gcs.chan_parameters[4],  gcs4,       "SR4_",     GCS_MAVLINK_Parameters),
+#endif
+
+#if MAVLINK_COMM_NUM_BUFFERS >= 6
+    // @Group: SR5_
+    // @Path: GCS_Mavlink.cpp
+    GOBJECTN(_gcs.chan_parameters[5],  gcs5,       "SR5_",     GCS_MAVLINK_Parameters),
+#endif
+
+#if MAVLINK_COMM_NUM_BUFFERS >= 7
+    // @Group: SR6_
+    // @Path: GCS_Mavlink.cpp
+    GOBJECTN(_gcs.chan_parameters[6],  gcs6,       "SR6_",     GCS_MAVLINK_Parameters),
+#endif
+
+    // @Group: AHRS_
+    // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
+    GOBJECT(ahrs,                   "AHRS_",    AP_AHRS),
+
+    // @Group: LOG
+    // @Path: ../libraries/AP_Logger/AP_Logger.cpp
+    GOBJECT(logger,           "LOG",  AP_Logger),
+
+    // @Group: BATT
+    // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
+    GOBJECT(battery,                "BATT",         AP_BattMonitor),
+
+    // @Group: BRD_
+    // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp
+    GOBJECT(BoardConfig,            "BRD_",       AP_BoardConfig),
+
+#if HAL_MAX_CAN_PROTOCOL_DRIVERS
+    // @Group: CAN_
+    // @Path: ../libraries/AP_CANManager/AP_CANManager.cpp
+    GOBJECT(can_mgr,        "CAN_",       AP_CANManager),
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    GOBJECT(sitl, "SIM_", SITL::SITL),
+#endif
+
+    // @Group: BARO
+    // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+    GOBJECT(barometer, "BARO", AP_Baro),
+
+    // GPS driver
+    // @Group: GPS_
+    // @Path: ../libraries/AP_GPS/AP_GPS.cpp
+    GOBJECT(gps, "GPS_", AP_GPS),
+
+    // @Group: SCHED_
+    // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
+    GOBJECT(scheduler, "SCHED_", AP_Scheduler),
+
+    // @Group: RCMAP_
+    // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
+    GOBJECT(rcmap, "RCMAP_",        RCMapper),
+
+#if HAL_NAVEKF2_AVAILABLE
+    // @Group: EK2_
+    // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp
+    GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2),
+#endif
+
+#if HAL_NAVEKF3_AVAILABLE
+    // @Group: EK3_
+    // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
+    GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),
+#endif
+
+    // @Group: RSSI_
+    // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
+    GOBJECT(rssi, "RSSI_",  AP_RSSI),
+
+    // @Group: NTF_
+    // @Path: ../libraries/AP_Notify/AP_Notify.cpp
+    GOBJECT(notify, "NTF_",  AP_Notify),
+
+    // @Group:
+    // @Path: Parameters.cpp
+    GOBJECT(g2, "",  ParametersG2),
+
+    // @Group: FINS_
+    // @Path: Fins.cpp
+    GOBJECTPTR(motors, "FINS_", Fins),
+
+    // @Group:
+    // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
+    { AP_PARAM_GROUP, "", Parameters::k_param_vehicle, (const void *)&blimp, {group_info : AP_Vehicle::var_info} },
+
+    AP_VAREND
+};
+
+/*
+  2nd group of parameters
+ */
+const AP_Param::GroupInfo ParametersG2::var_info[] = {
+
+    // @Param: WP_NAVALT_MIN
+    // @DisplayName: Minimum navigation altitude
+    // @Description: This is the altitude in meters above which for navigation can begin. This applies in auto takeoff and auto landing.
+    // @Range: 0 5
+    // @User: Standard
+    AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0),
+
+    // @Param: DEV_OPTIONS
+    // @DisplayName: Development options
+    // @Description: Bitmask of developer options. The meanings of the bit fields in this parameter may vary at any time. Developers should check the source code for current meaning
+    // @Bitmask: 0:ADSBMavlinkProcessing,1:DevOptionVFR_HUDRelativeAlt,2:SetAttitudeTarget_ThrustAsThrust
+    // @User: Advanced
+    AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0),
+
+    // @Param: ACRO_Y_EXPO
+    // @DisplayName: Acro Yaw Expo
+    // @Description: Acro yaw expo to allow faster rotation when stick at edges
+    // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
+    // @Range: -0.5 1.0
+    // @User: Advanced
+    AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT),
+
+
+    // @Param: SYSID_ENFORCE
+    // @DisplayName: GCS sysid enforcement
+    // @Description: This controls whether packets from other than the expected GCS system ID will be accepted
+    // @Values: 0:NotEnforced,1:Enforced
+    // @User: Advanced
+    AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0),
+
+#if STATS_ENABLED == ENABLED
+    // @Group: STAT
+    // @Path: ../libraries/AP_Stats/AP_Stats.cpp
+    AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats),
+#endif
+
+    // @Param: FRAME_CLASS
+    // @DisplayName: Frame Class
+    // @Description: Controls major frame class for multiblimp component
+    // @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 6:Heli, 7:Tri, 8:SingleCopter, 9:CoaxCopter, 10:BiCopter, 11:Heli_Dual, 12:DodecaHexa, 13:HeliQuad, 14:Deca
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS),
+
+    // @Group: SERVO
+    // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
+    AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels),
+
+    // @Group: RC
+    // @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h
+    AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels_Blimp),
+
+    // // 18 was used by AP_VisualOdom
+
+    // // @Group: TCAL
+    // // @Path: ../libraries/AP_TempCalibration/AP_TempCalibration.cpp
+    // AP_SUBGROUPINFO(temp_calibration, "TCAL", 19, ParametersG2, AP_TempCalibration),
+
+    // @Param: PILOT_SPEED_DN
+    // @DisplayName: Pilot maximum vertical speed descending
+    // @Description: The maximum vertical descending velocity the pilot may request in cm/s
+    // @Units: cm/s
+    // @Range: 50 500
+    // @Increment: 10
+    // @User: Standard
+    AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0),
+
+    // @Param: LAND_ALT_LOW
+    // @DisplayName: Land alt low
+    // @Description: Altitude during Landing at which vehicle slows to LAND_SPEED
+    // @Units: cm
+    // @Range: 100 10000
+    // @Increment: 10
+    // @User: Advanced
+    AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000),
+
+#ifdef ENABLE_SCRIPTING
+    // @Group: SCR_
+    // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp
+    AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting),
+#endif
+
+    // @Param: TUNE_MIN
+    // @DisplayName: Tuning minimum
+    // @Description: Minimum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to
+    // @User: Standard
+    AP_GROUPINFO("TUNE_MIN", 31, ParametersG2, tuning_min, 0),
+
+    // @Param: TUNE_MAX
+    // @DisplayName: Tuning maximum
+    // @Description: Maximum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to
+    // @User: Standard
+    AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0),
+
+    // @Param: FS_VIBE_ENABLE
+    // @DisplayName: Vibration Failsafe enable
+    // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Standard
+    AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),
+
+    // @Param: FS_OPTIONS
+    // @DisplayName: Failsafe options bitmask
+    // @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options.
+    // @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe
+    // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe, 5:Release Gripper
+    // @User: Advanced
+    AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, (float)Blimp::FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL),
+
+    // @Param: FS_GCS_TIMEOUT
+    // @DisplayName: GCS failsafe timeout
+    // @Description: Timeout before triggering the GCS failsafe
+    // @Units: s
+    // @Range: 2 120
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("FS_GCS_TIMEOUT", 42, ParametersG2, fs_gcs_timeout, 5),
+
+    AP_GROUPEND
+};
+
+/*
+  constructor for g2 object
+ */
+ParametersG2::ParametersG2(void)
+//    : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+
+void Blimp::load_parameters(void)
+{
+    if (!AP_Param::check_var_info()) {
+        hal.console->printf("Bad var table\n");
+        AP_HAL::panic("Bad var table");
+    }
+
+    // disable centrifugal force correction, it will be enabled as part of the arming process
+    ahrs.set_correct_centrifugal(false);
+    hal.util->set_soft_armed(false);
+
+    if (!g.format_version.load() ||
+        g.format_version != Parameters::k_format_version) {
+
+        // erase all parameters
+        hal.console->printf("Firmware change: erasing EEPROM...\n");
+        StorageManager::erase();
+        AP_Param::erase_all();
+
+        // save the current format version
+        g.format_version.set_and_save(Parameters::k_format_version);
+        hal.console->printf("done.\n");
+    }
+
+    uint32_t before = micros();
+    // Load all auto-loaded EEPROM variables
+    AP_Param::load_all();
+
+    hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
+
+    // setup AP_Param frame type flags
+    AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER);
+
+}

Blimp/Parameters.h

@@ -0,0 +1,529 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include "RC_Channel.h"
+
+// Global parameter class.
+//
+class Parameters
+{
+public:
+    // The version of the layout as described by the parameter enum.
+    //
+    // When changing the parameter enum in an incompatible fashion, this
+    // value should be incremented by one.
+    //
+    // The increment will prevent old parameters from being used incorrectly
+    // by newer code.
+    //
+    static const uint16_t        k_format_version = 120;
+
+    // Parameter identities.
+    //
+    // The enumeration defined here is used to ensure that every parameter
+    // or parameter group has a unique ID number.  This number is used by
+    // AP_Param to store and locate parameters in EEPROM.
+    //
+    // Note that entries without a number are assigned the next number after
+    // the entry preceding them. When adding new entries, ensure that they
+    // don't overlap.
+    //
+    // Try to group related variables together, and assign them a set
+    // range in the enumeration. Place these groups in numerical order
+    // at the end of the enumeration.
+    //
+    // WARNING: Care should be taken when editing this enumeration as the
+    //          AP_Param load/save code depends on the values here to identify
+    //          variables saved in EEPROM.
+    //
+    //
+    enum {
+        // Layout version number, always key zero.
+        //
+        k_param_format_version = 0,
+        k_param_software_type, // deprecated
+        k_param_ins_old,                        // *** Deprecated, remove with next eeprom number change
+        k_param_ins,                            // libraries/AP_InertialSensor variables
+        k_param_NavEKF2_old, // deprecated - remove
+        k_param_NavEKF2,
+        k_param_g2, // 2nd block of parameters
+        k_param_NavEKF3,
+        k_param_can_mgr,
+        k_param_osd,
+
+        // simulation
+        k_param_sitl = 10,
+
+        // barometer object (needed for SITL)
+        k_param_barometer,
+
+        // scheduler object (for debugging)
+        k_param_scheduler,
+
+        // relay object
+        k_param_relay,
+
+        // (old) EPM object
+        k_param_epm_unused,
+
+        // BoardConfig object
+        k_param_BoardConfig,
+
+        // GPS object
+        k_param_gps,
+
+        // Parachute object
+        k_param_parachute,
+
+        // Landing gear object
+        k_param_landinggear,    // 18
+
+        // Input Management object
+        k_param_input_manager,  // 19
+
+        // Misc
+        //
+        k_param_log_bitmask_old = 20,           // Deprecated
+        k_param_log_last_filenumber,            // *** Deprecated - remove
+        // with next eeprom number
+        // change
+        k_param_toy_yaw_rate,                   // deprecated - remove
+        k_param_crosstrack_min_distance,    // deprecated - remove with next eeprom number change
+        k_param_rssi_pin,                   // unused, replaced by rssi_ library parameters
+        k_param_throttle_accel_enabled,     // deprecated - remove
+        k_param_wp_yaw_behavior,
+        k_param_acro_trainer,
+        k_param_pilot_speed_up,    // renamed from k_param_pilot_velocity_z_max
+        k_param_circle_rate,                // deprecated - remove
+        k_param_rangefinder_gain,
+        k_param_ch8_option_old, // deprecated
+        k_param_arming_check_old,       // deprecated - remove
+        k_param_sprayer,
+        k_param_angle_max,
+        k_param_gps_hdop_good,
+        k_param_battery,
+        k_param_fs_batt_mah,            // unused - moved to AP_BattMonitor
+        k_param_angle_rate_max,         // remove
+        k_param_rssi_range,             // unused, replaced by rssi_ library parameters
+        k_param_rc_feel_rp,             // deprecated
+        k_param_NavEKF,                 // deprecated - remove
+        k_param_mission,                // mission library
+        k_param_rc_13_old,
+        k_param_rc_14_old,
+        k_param_rally,
+        k_param_poshold_brake_rate,
+        k_param_poshold_brake_angle_max,
+        k_param_pilot_accel_z,
+        k_param_serial0_baud,           // deprecated - remove
+        k_param_serial1_baud,           // deprecated - remove
+        k_param_serial2_baud,           // deprecated - remove
+        k_param_land_repositioning,
+        k_param_rangefinder, // rangefinder object
+        k_param_fs_ekf_thresh,
+        k_param_terrain,
+        k_param_acro_rp_expo,
+        k_param_throttle_deadzone,
+        k_param_optflow,
+        k_param_dcmcheck_thresh,        // deprecated - remove
+        k_param_log_bitmask,
+        k_param_cli_enabled_old,        // deprecated - remove
+        k_param_throttle_filt,
+        k_param_throttle_behavior,
+        k_param_pilot_takeoff_alt, // 64
+
+        // 65: AP_Limits Library
+        k_param_limits = 65,            // deprecated - remove
+        k_param_gpslock_limit,          // deprecated - remove
+        k_param_geofence_limit,         // deprecated - remove
+        k_param_altitude_limit,         // deprecated - remove
+        k_param_fence,
+        k_param_gps_glitch,             // deprecated
+        k_param_baro_glitch,            // 71 - deprecated
+
+        // AP_ADSB Library
+        k_param_adsb,                   // 72
+        k_param_notify,                 // 73
+
+        // 74: precision landing object
+        k_param_precland = 74,
+
+        //
+        // 75: Singlecopter, CoaxBlimp
+        //
+        k_param_single_servo_1 = 75,    // remove
+        k_param_single_servo_2,         // remove
+        k_param_single_servo_3,         // remove
+        k_param_single_servo_4,         // 78 - remove
+
+        //
+        // 80: Heli
+        //
+        k_param_heli_servo_1 = 80,  // remove
+        k_param_heli_servo_2,       // remove
+        k_param_heli_servo_3,       // remove
+        k_param_heli_servo_4,       // remove
+        k_param_heli_pitch_ff,      // remove
+        k_param_heli_roll_ff,       // remove
+        k_param_heli_yaw_ff,        // remove
+        k_param_heli_stab_col_min,  // remove
+        k_param_heli_stab_col_max,  // remove
+        k_param_heli_servo_rsc,     // 89 = full! - remove
+
+        //
+        // 90: misc2
+        //
+        k_param_motors = 90,
+        k_param_disarm_delay,
+        k_param_fs_crash_check,
+        k_param_throw_motor_start,
+        k_param_rtl_alt_type,
+        k_param_avoid,
+        k_param_avoidance_adsb,
+
+        // 97: RSSI
+        k_param_rssi = 97,
+
+        //
+        // 100: Inertial Nav
+        //
+        k_param_inertial_nav = 100, // deprecated
+        k_param_wp_nav,
+        k_param_attitude_control,
+        k_param_pos_control,
+        k_param_circle_nav,
+        k_param_loiter_nav,     // 105
+
+        // 110: Telemetry control
+        //
+        k_param_gcs0 = 110,
+        k_param_gcs1,
+        k_param_sysid_this_mav,
+        k_param_sysid_my_gcs,
+        k_param_serial1_baud_old, // deprecated
+        k_param_telem_delay,
+        k_param_gcs2,
+        k_param_serial2_baud_old, // deprecated
+        k_param_serial2_protocol, // deprecated
+        k_param_serial_manager,
+        k_param_ch9_option_old,
+        k_param_ch10_option_old,
+        k_param_ch11_option_old,
+        k_param_ch12_option_old,
+        k_param_takeoff_trigger_dz_old,
+        k_param_gcs3,
+        k_param_gcs_pid_mask,    // 126
+        k_param_gcs4,
+        k_param_gcs5,
+        k_param_gcs6,
+
+        //
+        // 135 : reserved for Solo until features merged with master
+        //
+        k_param_rtl_speed_cms = 135,
+        k_param_fs_batt_curr_rtl,
+        k_param_rtl_cone_slope, // 137
+
+        //
+        // 140: Sensor parameters
+        //
+        k_param_imu = 140, // deprecated - can be deleted
+        k_param_battery_monitoring = 141,   // deprecated - can be deleted
+        k_param_volt_div_ratio, // deprecated - can be deleted
+        k_param_curr_amp_per_volt,  // deprecated - can be deleted
+        k_param_input_voltage,  // deprecated - can be deleted
+        k_param_pack_capacity,  // deprecated - can be deleted
+        k_param_compass_enabled_deprecated,
+        k_param_compass,
+        k_param_rangefinder_enabled_old, // deprecated
+        k_param_frame_type,
+        k_param_optflow_enabled,    // deprecated
+        k_param_fs_batt_voltage,    // unused - moved to AP_BattMonitor
+        k_param_ch7_option_old,
+        k_param_auto_slew_rate,     // deprecated - can be deleted
+        k_param_rangefinder_type_old,     // deprecated
+        k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
+        k_param_copter_leds_mode,   // deprecated - remove with next eeprom number change
+        k_param_ahrs, // AHRS group // 159
+
+        //
+        // 160: Navigation parameters
+        //
+        k_param_rtl_altitude = 160,
+        k_param_crosstrack_gain,    // deprecated - remove with next eeprom number change
+        k_param_rtl_loiter_time,
+        k_param_rtl_alt_final,
+        k_param_tilt_comp, // 164 deprecated - remove with next eeprom number change
+
+
+        //
+        // Camera and mount parameters
+        //
+        k_param_camera = 165,
+        k_param_camera_mount,
+        k_param_camera_mount2,      // deprecated
+
+        //
+        // Battery monitoring parameters
+        //
+        k_param_battery_volt_pin = 168, // deprecated - can be deleted
+        k_param_battery_curr_pin,   // 169 deprecated - can be deleted
+
+        //
+        // 170: Radio settings
+        //
+        k_param_rc_1_old = 170,
+        k_param_rc_2_old,
+        k_param_rc_3_old,
+        k_param_rc_4_old,
+        k_param_rc_5_old,
+        k_param_rc_6_old,
+        k_param_rc_7_old,
+        k_param_rc_8_old,
+        k_param_rc_10_old,
+        k_param_rc_11_old,
+        k_param_throttle_min,           // remove
+        k_param_throttle_max,           // remove
+        k_param_failsafe_throttle,
+        k_param_throttle_fs_action,     // remove
+        k_param_failsafe_throttle_value,
+        k_param_throttle_trim,          // remove
+        k_param_radio_tuning,
+        k_param_radio_tuning_high_old,   // unused
+        k_param_radio_tuning_low_old,    // unused
+        k_param_rc_speed = 192,
+        k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor
+        k_param_throttle_mid,           // remove
+        k_param_failsafe_gps_enabled,   // remove
+        k_param_rc_9_old,
+        k_param_rc_12_old,
+        k_param_failsafe_gcs,
+        k_param_rcmap, // 199
+
+        //
+        // 200: flight modes
+        //
+        k_param_flight_mode1 = 200,
+        k_param_flight_mode2,
+        k_param_flight_mode3,
+        k_param_flight_mode4,
+        k_param_flight_mode5,
+        k_param_flight_mode6,
+        k_param_flight_mode_chan,
+        k_param_initial_mode,
+
+        //
+        // 210: Waypoint data
+        //
+        k_param_waypoint_mode = 210, // remove
+        k_param_command_total,       // remove
+        k_param_command_index,       // remove
+        k_param_command_nav_index,   // remove
+        k_param_waypoint_radius,     // remove
+        k_param_circle_radius,       // remove
+        k_param_waypoint_speed_max,  // remove
+        k_param_land_speed,
+        k_param_auto_velocity_z_min, // remove
+        k_param_auto_velocity_z_max, // remove - 219
+        k_param_land_speed_high,
+
+        //
+        // 220: PI/D Controllers
+        //
+        k_param_acro_rp_p = 221,
+        k_param_axis_lock_p,    // remove
+        k_param_pid_rate_roll,      // remove
+        k_param_pid_rate_pitch,     // remove
+        k_param_pid_rate_yaw,       // remove
+        k_param_p_stabilize_roll,   // remove
+        k_param_p_stabilize_pitch,  // remove
+        k_param_p_stabilize_yaw,    // remove
+        k_param_p_pos_xy,           // remove
+        k_param_p_loiter_lon,       // remove
+        k_param_pid_loiter_rate_lat,    // remove
+        k_param_pid_loiter_rate_lon,    // remove
+        k_param_pid_nav_lat,        // remove
+        k_param_pid_nav_lon,        // remove
+        k_param_p_alt_hold,             // remove
+        k_param_p_vel_z,                // remove
+        k_param_pid_optflow_roll,       // remove
+        k_param_pid_optflow_pitch,      // remove
+        k_param_acro_balance_roll_old,  // remove
+        k_param_acro_balance_pitch_old, // remove
+        k_param_pid_accel_z,            // remove
+        k_param_acro_balance_roll,
+        k_param_acro_balance_pitch,
+        k_param_acro_yaw_p,
+        k_param_autotune_axis_bitmask, // remove
+        k_param_autotune_aggressiveness, // remove
+        k_param_pi_vel_xy,              // remove
+        k_param_fs_ekf_action,
+        k_param_rtl_climb_min,
+        k_param_rpm_sensor,
+        k_param_autotune_min_d, // remove
+        k_param_arming, // 252  - AP_Arming
+        k_param_logger = 253, // 253 - Logging Group
+
+        // 254,255: reserved
+
+        k_param_vehicle = 257, // vehicle common block of parameters
+
+        // the k_param_* space is 9-bits in size
+        // 511: reserved
+    };
+
+    AP_Int16        format_version;
+
+    // Telemetry control
+    //
+    AP_Int16        sysid_this_mav;
+    AP_Int16        sysid_my_gcs;
+    AP_Int8         telem_delay;
+
+    AP_Float        throttle_filt;
+    AP_Int16        throttle_behavior;
+    AP_Float        pilot_takeoff_alt;
+
+    AP_Int16        rtl_altitude;
+    AP_Int16        rtl_speed_cms;
+    AP_Float        rtl_cone_slope;
+
+    AP_Int8         failsafe_gcs;               // ground station failsafe behavior
+    AP_Int16        gps_hdop_good;              // GPS Hdop value at or below this value represent a good position
+
+    AP_Int16        rtl_alt_final;
+    AP_Int16        rtl_climb_min;              // rtl minimum climb in cm
+
+    AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
+
+    AP_Int16        poshold_brake_rate;         // PosHold flight mode's rotation rate during braking in deg/sec
+    AP_Int16        poshold_brake_angle_max;    // PosHold flight mode's max lean angle during braking in centi-degrees
+
+    // Waypoints
+    //
+    AP_Int32        rtl_loiter_time;
+    AP_Int16        land_speed;
+    AP_Int16        land_speed_high;
+    AP_Int16        pilot_speed_up;    // maximum vertical ascending velocity the pilot may request
+    AP_Int16        pilot_accel_z;               // vertical acceleration the pilot may request
+
+    // Throttle
+    //
+    AP_Int8         failsafe_throttle;
+    AP_Int16        failsafe_throttle_value;
+    AP_Int16        throttle_deadzone;
+
+    // Flight modes
+    //
+    AP_Int8         flight_mode1;
+    AP_Int8         flight_mode2;
+    AP_Int8         flight_mode3;
+    AP_Int8         flight_mode4;
+    AP_Int8         flight_mode5;
+    AP_Int8         flight_mode6;
+    AP_Int8         flight_mode_chan;
+    AP_Int8         initial_mode;
+
+    // Misc
+    //
+    AP_Int32        log_bitmask;
+    AP_Int8         radio_tuning;
+    AP_Int8         frame_type;
+    AP_Int8         disarm_delay;
+
+    AP_Int8         land_repositioning;
+    AP_Int8         fs_ekf_action;
+    AP_Int8         fs_crash_check;
+    AP_Float        fs_ekf_thresh;
+    AP_Int16        gcs_pid_mask;
+
+    AP_Int8         rtl_alt_type;
+
+    AP_Int16                rc_speed; // speed of fast RC Channels in Hz
+
+    // Acro parameters
+    AP_Float                acro_rp_p;
+    AP_Float                acro_yaw_p;
+    AP_Float                acro_balance_roll;
+    AP_Float                acro_balance_pitch;
+    AP_Int8                 acro_trainer;
+    AP_Float                acro_rp_expo;
+
+    // Note: keep initializers here in the same order as they are declared
+    // above.
+    Parameters()
+    {
+    }
+};
+
+/*
+  2nd block of parameters, to avoid going past 256 top level keys
+ */
+class ParametersG2
+{
+public:
+    ParametersG2(void);
+
+    // var_info for holding Parameter information
+    static const struct AP_Param::GroupInfo var_info[];
+
+    // altitude at which nav control can start in takeoff
+    AP_Float wp_navalt_min;
+
+    // // button checking
+    // AP_Button *button_ptr;
+
+#if STATS_ENABLED == ENABLED
+    // vehicle statistics
+    AP_Stats stats;
+#endif
+
+
+    // ground effect compensation enable/disable
+    // AP_Int8 gndeffect_comp_enabled;
+
+    // temperature calibration handling
+    // AP_TempCalibration temp_calibration;
+
+
+    // whether to enforce acceptance of packets only from sysid_my_gcs
+    AP_Int8 sysid_enforce;
+
+    // developer options
+    AP_Int32 dev_options;
+
+    // acro exponent parameters
+    AP_Float acro_y_expo;
+
+    // frame class
+    AP_Int8 frame_class;
+
+    // RC input channels
+    RC_Channels_Blimp rc_channels;
+
+    // control over servo output ranges
+    SRV_Channels servo_channels;
+
+    // Additional pilot velocity items
+    AP_Int16    pilot_speed_dn;
+
+    // Land alt final stage
+    AP_Int16 land_alt_low;
+
+
+#ifdef ENABLE_SCRIPTING
+    AP_Scripting scripting;
+#endif // ENABLE_SCRIPTING
+
+    AP_Float tuning_min;
+    AP_Float tuning_max;
+
+    // vibration failsafe enable/disable
+    AP_Int8 fs_vibe_enabled;
+
+    // Failsafe options bitmask #36
+    AP_Int32 fs_options;
+
+    AP_Float fs_gcs_timeout;
+};
+
+extern const AP_Param::Info        var_info[];

Blimp/RC_Channel.cpp

@@ -0,0 +1,255 @@
+#include "Blimp.h"
+
+#include "RC_Channel.h"
+
+
+// defining these two macros and including the RC_Channels_VarInfo header defines the parameter information common to all vehicle types
+#define RC_CHANNELS_SUBCLASS RC_Channels_Blimp
+#define RC_CHANNEL_SUBCLASS RC_Channel_Blimp
+
+#include <RC_Channel/RC_Channels_VarInfo.h>
+
+int8_t RC_Channels_Blimp::flight_mode_channel_number() const
+{
+    return blimp.g.flight_mode_chan.get();
+}
+
+void RC_Channel_Blimp::mode_switch_changed(modeswitch_pos_t new_pos)
+{
+    if (new_pos < 0 || (uint8_t)new_pos > blimp.num_flight_modes) {
+        // should not have been called
+        return;
+    }
+
+    if (!blimp.set_mode((Mode::Number)blimp.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) {
+        // alert user to mode change failure
+        if (blimp.ap.initialised) {
+            AP_Notify::events.user_mode_change_failed = 1;
+        }
+        return;
+    }
+
+    // play a tone
+    // alert user to mode change (except if autopilot is just starting up)
+    if (blimp.ap.initialised) {
+        AP_Notify::events.user_mode_change = 1;
+    }
+}
+
+bool RC_Channels_Blimp::has_valid_input() const
+{
+    if (blimp.failsafe.radio) {
+        return false;
+    }
+    if (blimp.failsafe.radio_counter != 0) {
+        return false;
+    }
+    return true;
+}
+
+RC_Channel * RC_Channels_Blimp::get_arming_channel(void) const
+{
+    return blimp.channel_yaw;
+}
+
+// init_aux_switch_function - initialize aux functions
+void RC_Channel_Blimp::init_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
+{
+    // init channel options
+    switch (ch_option) {
+    // the following functions do not need to be initialised:
+    case AUX_FUNC::MANUAL:
+        break;
+    default:
+        RC_Channel::init_aux_function(ch_option, ch_flag);
+        break;
+    }
+}
+
+// do_aux_function_change_mode - change mode based on an aux switch
+// being moved
+void RC_Channel_Blimp::do_aux_function_change_mode(const Mode::Number mode,
+        const AuxSwitchPos ch_flag)
+{
+    switch (ch_flag) {
+    case AuxSwitchPos::HIGH: {
+        // engage mode (if not possible we remain in current flight mode)
+        const bool success = blimp.set_mode(mode, ModeReason::RC_COMMAND);
+        if (blimp.ap.initialised) {
+            if (success) {
+                AP_Notify::events.user_mode_change = 1;
+            } else {
+                AP_Notify::events.user_mode_change_failed = 1;
+            }
+        }
+        break;
+    }
+    default:
+        // return to flight mode switch's flight mode if we are currently
+        // in this mode
+        if (blimp.control_mode == mode) {
+            rc().reset_mode_switch();
+        }
+    }
+}
+
+void RC_Channel_Blimp::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
+{
+    RC_Channel::do_aux_function_armdisarm(ch_flag);
+    if (blimp.arming.is_armed()) {
+        // remember that we are using an arming switch, for use by
+        // set_throttle_zero_flag
+        blimp.ap.armed_with_switch = true;
+    }
+}
+
+// do_aux_function - implement the function invoked by auxiliary switches
+void RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
+{
+    switch (ch_option) {
+
+    case AUX_FUNC::SAVE_TRIM:
+        if ((ch_flag == AuxSwitchPos::HIGH) &&
+            (blimp.control_mode <= Mode::Number::MANUAL) &&
+            (blimp.channel_down->get_control_in() == 0)) {
+            blimp.save_trim();
+        }
+        break;
+
+    //         case AUX_FUNC::SAVE_WP:
+    // #if MODE_AUTO_ENABLED == ENABLED
+    //             // save waypoint when switch is brought high
+    //             if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) {
+
+    //                 // do not allow saving new waypoints while we're in auto or disarmed
+    //                 if (blimp.control_mode == Mode::Number::AUTO || !blimp.motors->armed()) {
+    //                     return;
+    //                 }
+
+    //                 // do not allow saving the first waypoint with zero throttle
+    //                 if ((blimp.mode_auto.mission.num_commands() == 0) && (blimp.channel_down->get_control_in() == 0)) {
+    //                     return;
+    //                 }
+
+    //                 // create new mission command
+    //                 AP_Mission::Mission_Command cmd  = {};
+
+    //                 // if the mission is empty save a takeoff command
+    //                 if (blimp.mode_auto.mission.num_commands() == 0) {
+    //                     // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
+    //                     cmd.id = MAV_CMD_NAV_TAKEOFF;
+    //                     cmd.content.location.alt = MAX(blimp.current_loc.alt,100);
+
+    //                     // use the current altitude for the target alt for takeoff.
+    //                     // only altitude will matter to the AP mission script for takeoff.
+    //                     if (blimp.mode_auto.mission.add_cmd(cmd)) {
+    //                         // log event
+    //                         AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP);
+    //                     }
+    //                 }
+
+    //                 // set new waypoint to current location
+    //                 cmd.content.location = blimp.current_loc;
+
+    //                 // if throttle is above zero, create waypoint command
+    //                 if (blimp.channel_down->get_control_in() > 0) {
+    //                     cmd.id = MAV_CMD_NAV_WAYPOINT;
+    //                 } else {
+    //                     // with zero throttle, create LAND command
+    //                     cmd.id = MAV_CMD_NAV_LAND;
+    //                 }
+
+    //                 // save command
+    //                 if (blimp.mode_auto.mission.add_cmd(cmd)) {
+    //                     // log event
+    //                     AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP);
+    //                 }
+    //             }
+    // #endif
+    //              break;
+
+    //         case AUX_FUNC::AUTO:
+    // #if MODE_AUTO_ENABLED == ENABLED
+    //             do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
+    // #endif
+    //             break;
+
+    // case AUX_FUNC::LOITER:
+    //     do_aux_function_change_mode(Mode::Number::LOITER, ch_flag);
+    //     break;
+
+    case AUX_FUNC::MANUAL:
+        do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag);
+        break;
+
+    default:
+        RC_Channel::do_aux_function(ch_option, ch_flag);
+        break;
+    }
+}
+
+// save_trim - adds roll and pitch trims from the radio to ahrs
+void Blimp::save_trim()
+{
+    // save roll and pitch trim
+    float roll_trim = ToRad((float)channel_right->get_control_in()/100.0f);
+    float pitch_trim = ToRad((float)channel_front->get_control_in()/100.0f);
+    ahrs.add_trim(roll_trim, pitch_trim);
+    AP::logger().Write_Event(LogEvent::SAVE_TRIM);
+    gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");
+}
+
+// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
+// meant to be called continuously while the pilot attempts to keep the blimp level
+void Blimp::auto_trim_cancel()
+{
+    auto_trim_counter = 0;
+    AP_Notify::flags.save_trim = false;
+    gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled");
+}
+
+void Blimp::auto_trim()
+{
+    if (auto_trim_counter > 0) {
+        if (blimp.flightmode != &blimp.mode_manual ||
+            !blimp.motors->armed()) {
+            auto_trim_cancel();
+            return;
+        }
+
+        // flash the leds
+        AP_Notify::flags.save_trim = true;
+
+        if (!auto_trim_started) {
+            if (ap.land_complete) {
+                // haven't taken off yet
+                return;
+            }
+            auto_trim_started = true;
+        }
+
+        if (ap.land_complete) {
+            // landed again.
+            auto_trim_cancel();
+            return;
+        }
+
+        auto_trim_counter--;
+
+        // calculate roll trim adjustment
+        float roll_trim_adjustment = ToRad((float)channel_right->get_control_in() / 4000.0f);
+
+        // calculate pitch trim adjustment
+        float pitch_trim_adjustment = ToRad((float)channel_front->get_control_in() / 4000.0f);
+
+        // add trim to ahrs object
+        // save to eeprom on last iteration
+        ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
+
+        // on last iteration restore leds and accel gains to normal
+        if (auto_trim_counter == 0) {
+            AP_Notify::flags.save_trim = false;
+            gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim: Trims saved");
+        }
+    }
+}

Blimp/RC_Channel.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <RC_Channel/RC_Channel.h>
+#include "Fins.h"
+#include "mode.h" //this includes Blimp.h which includes Fins.h
+
+class RC_Channel_Blimp : public RC_Channel
+{
+
+public:
+
+protected:
+
+    void init_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
+    void do_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
+
+private:
+
+    void do_aux_function_armdisarm(const AuxSwitchPos ch_flag) override;
+    void do_aux_function_change_mode(const Mode::Number mode,
+                                     const AuxSwitchPos ch_flag);
+    void do_aux_function_change_air_mode(const AuxSwitchPos ch_flag);
+
+    // called when the mode switch changes position:
+    void mode_switch_changed(modeswitch_pos_t new_pos) override;
+
+};
+
+class RC_Channels_Blimp : public RC_Channels
+{
+public:
+
+    bool has_valid_input() const override;
+
+    RC_Channel *get_arming_channel(void) const override;
+
+    RC_Channel_Blimp obj_channels[NUM_RC_CHANNELS];
+    RC_Channel_Blimp *channel(const uint8_t chan) override
+    {
+        if (chan >= NUM_RC_CHANNELS) {
+            return nullptr;
+        }
+        return &obj_channels[chan];
+    }
+
+protected:
+
+    int8_t flight_mode_channel_number() const override;
+
+};

Blimp/commands.cpp

@@ -0,0 +1,110 @@
+#include "Blimp.h"
+
+// checks if we should update ahrs/RTL home position from the EKF
+void Blimp::update_home_from_EKF()
+{
+    // exit immediately if home already set
+    if (ahrs.home_is_set()) {
+        return;
+    }
+
+    // special logic if home is set in-flight
+    if (motors->armed()) {
+        set_home_to_current_location_inflight();
+    } else {
+        // move home to current ekf location (this will set home_state to HOME_SET)
+        if (!set_home_to_current_location(false)) {
+            // ignore failure
+        }
+    }
+}
+
+// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
+void Blimp::set_home_to_current_location_inflight()
+{
+    // get current location from EKF
+    Location temp_loc;
+    Location ekf_origin;
+    if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) {
+        temp_loc.alt = ekf_origin.alt;
+        if (!set_home(temp_loc, false)) {
+            return;
+        }
+    }
+}
+
+// set_home_to_current_location - set home to current GPS location
+bool Blimp::set_home_to_current_location(bool lock)
+{
+    // get current location from EKF
+    Location temp_loc;
+    if (ahrs.get_location(temp_loc)) {
+        if (!set_home(temp_loc, lock)) {
+            return false;
+        }
+        return true;
+    }
+    return false;
+}
+
+// set_home - sets ahrs home (used for RTL) to specified location
+//  initialises inertial nav and compass on first call
+//  returns true if home location set successfully
+bool Blimp::set_home(const Location& loc, bool lock)
+{
+    // check EKF origin has been set
+    Location ekf_origin;
+    if (!ahrs.get_origin(ekf_origin)) {
+        return false;
+    }
+
+    // check home is close to EKF origin
+    if (far_from_EKF_origin(loc)) {
+        return false;
+    }
+
+    const bool home_was_set = ahrs.home_is_set();
+
+    // set ahrs home (used for RTL)
+    if (!ahrs.set_home(loc)) {
+        return false;
+    }
+
+    // init inav and compass declination
+    if (!home_was_set) {
+        // record home is set
+        AP::logger().Write_Event(LogEvent::SET_HOME);
+
+        // #if MODE_AUTO_ENABLED == ENABLED
+        //         // log new home position which mission library will pull from ahrs
+        //         if (should_log(MASK_LOG_CMD)) {
+        //             AP_Mission::Mission_Command temp_cmd;
+        //             if (mode_auto.mission.read_cmd_from_storage(0, temp_cmd)) {
+        //                 logger.Write_Mission_Cmd(mode_auto.mission, temp_cmd);
+        //             }
+        //         }
+        // #endif
+    }
+
+    // lock home position
+    if (lock) {
+        ahrs.lock_home();
+    }
+
+    // return success
+    return true;
+}
+
+// far_from_EKF_origin - checks if a location is too far from the EKF origin
+//  returns true if too far
+bool Blimp::far_from_EKF_origin(const Location& loc)
+{
+    // check distance to EKF origin
+    Location ekf_origin;
+    if (ahrs.get_origin(ekf_origin) && ((ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M) || (labs(ekf_origin.alt - loc.alt) > EKF_ORIGIN_MAX_DIST_M))) {
+        return true;
+    }
+
+    // close enough to origin
+    return false;
+}

Blimp/config.h

@@ -0,0 +1,593 @@
+//
+#pragma once
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Default and automatic configuration details.
+//
+//
+#include "defines.h"
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+// HARDWARE CONFIGURATION AND CONNECTIONS
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef CONFIG_HAL_BOARD
+#error CONFIG_HAL_BOARD must be defined to build Blimp
+#endif
+
+#ifndef ADVANCED_FAILSAFE
+# define ADVANCED_FAILSAFE DISABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// HIL_MODE                                 OPTIONAL
+
+#ifndef HIL_MODE
+#define HIL_MODE        HIL_MODE_DISABLED
+#endif
+
+#ifndef ARMING_DELAY_SEC
+# define ARMING_DELAY_SEC 2.0f
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// FRAME_CONFIG
+//
+#ifndef FRAME_CONFIG
+# define FRAME_CONFIG   MULTICOPTER_FRAME
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// PWM control
+// default RC speed in Hz
+#ifndef RC_FAST_SPEED
+#   define RC_FAST_SPEED 490
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Rangefinder
+//
+
+#ifndef RANGEFINDER_ENABLED
+# define RANGEFINDER_ENABLED ENABLED
+#endif
+
+#ifndef RANGEFINDER_HEALTH_MAX
+# define RANGEFINDER_HEALTH_MAX 3          // number of good reads that indicates a healthy rangefinder
+#endif
+
+#ifndef RANGEFINDER_TIMEOUT_MS
+# define RANGEFINDER_TIMEOUT_MS 1000        // rangefinder filter reset if no updates from sensor in 1 second
+#endif
+
+#ifndef RANGEFINDER_GAIN_DEFAULT
+# define RANGEFINDER_GAIN_DEFAULT 0.8f     // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
+#endif
+
+#ifndef SURFACE_TRACKING_VELZ_MAX
+# define SURFACE_TRACKING_VELZ_MAX 150     // max vertical speed change while surface tracking with rangefinder
+#endif
+
+#ifndef SURFACE_TRACKING_TIMEOUT_MS
+# define SURFACE_TRACKING_TIMEOUT_MS  1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
+#endif
+
+#ifndef RANGEFINDER_WPNAV_FILT_HZ
+# define RANGEFINDER_WPNAV_FILT_HZ   0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class
+#endif
+
+#ifndef RANGEFINDER_TILT_CORRECTION         // by disable tilt correction for use of range finder data by EKF
+# define RANGEFINDER_TILT_CORRECTION ENABLED
+#endif
+
+#ifndef RANGEFINDER_GLITCH_ALT_CM
+# define RANGEFINDER_GLITCH_ALT_CM  200      // amount of rangefinder change to be considered a glitch
+#endif
+
+#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES
+# define RANGEFINDER_GLITCH_NUM_SAMPLES  3   // number of rangefinder glitches in a row to take new reading
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Proximity sensor
+//
+#ifndef PROXIMITY_ENABLED
+# define PROXIMITY_ENABLED ENABLED
+#endif
+
+#ifndef MAV_SYSTEM_ID
+# define MAV_SYSTEM_ID          1
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Battery monitoring
+//
+#ifndef BOARD_VOLTAGE_MIN
+# define BOARD_VOLTAGE_MIN             4.3f        // min board voltage in volts for pre-arm checks
+#endif
+
+#ifndef BOARD_VOLTAGE_MAX
+# define BOARD_VOLTAGE_MAX             5.8f        // max board voltage in volts for pre-arm checks
+#endif
+
+// prearm GPS hdop check
+#ifndef GPS_HDOP_GOOD_DEFAULT
+# define GPS_HDOP_GOOD_DEFAULT         140     // minimum hdop that represents a good position.  used during pre-arm checks if fence is enabled
+#endif
+
+// GCS failsafe
+#ifndef FS_GCS
+# define FS_GCS                        DISABLED
+#endif
+
+// Radio failsafe while using RC_override
+#ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS
+# define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS  1000    // RC Radio failsafe triggers after 1 second while using RC_override from ground station
+#endif
+
+// Radio failsafe
+#ifndef FS_RADIO_TIMEOUT_MS
+#define FS_RADIO_TIMEOUT_MS            500     // RC Radio Failsafe triggers after 500 milliseconds with No RC Input
+#endif
+
+// missing terrain data failsafe
+#ifndef FS_TERRAIN_TIMEOUT_MS
+#define FS_TERRAIN_TIMEOUT_MS          5000     // 5 seconds of missing terrain data will trigger failsafe (RTL)
+#endif
+
+#ifndef PREARM_DISPLAY_PERIOD
+# define PREARM_DISPLAY_PERIOD 30
+#endif
+
+// pre-arm baro vs inertial nav max alt disparity
+#ifndef PREARM_MAX_ALT_DISPARITY_CM
+# define PREARM_MAX_ALT_DISPARITY_CM       100     // barometer and inertial nav altitude must be within this many centimeters
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+//  EKF Failsafe
+#ifndef FS_EKF_ACTION_DEFAULT
+# define FS_EKF_ACTION_DEFAULT         FS_EKF_ACTION_LAND  // EKF failsafe triggers land by default
+#endif
+#ifndef FS_EKF_THRESHOLD_DEFAULT
+# define FS_EKF_THRESHOLD_DEFAULT      0.8f    // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
+#endif
+
+#ifndef EKF_ORIGIN_MAX_DIST_M
+# define EKF_ORIGIN_MAX_DIST_M         50000   // EKF origin and waypoints (including home) must be within 50km
+#endif
+
+#ifndef COMPASS_CAL_STICK_GESTURE_TIME
+#define COMPASS_CAL_STICK_GESTURE_TIME 2.0f // 2 seconds
+#endif
+#ifndef COMPASS_CAL_STICK_DELAY
+#define COMPASS_CAL_STICK_DELAY 5.0f
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+//  OPTICAL_FLOW
+#ifndef OPTFLOW
+# define OPTFLOW       ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+//  Auto Tuning
+#ifndef AUTOTUNE_ENABLED
+# define AUTOTUNE_ENABLED  ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+//  Crop Sprayer - enabled only on larger firmwares
+#ifndef SPRAYER_ENABLED
+# define SPRAYER_ENABLED  HAL_SPRAYER_ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Precision Landing with companion computer or IRLock sensor
+#ifndef PRECISION_LANDING
+# define PRECISION_LANDING ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// gripper - enabled only on larger firmwares
+#ifndef GRIPPER_ENABLED
+# define GRIPPER_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// rotations per minute sensor support
+#ifndef RPM_ENABLED
+# define RPM_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Parachute release
+#ifndef PARACHUTE
+# define PARACHUTE HAL_PARACHUTE_ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Nav-Guided - allows external nav computer to control vehicle
+#ifndef NAV_GUIDED
+# define NAV_GUIDED    !HAL_MINIMIZE_FEATURES
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Acro - fly vehicle in acrobatic mode
+#ifndef MODE_ACRO_ENABLED
+# define MODE_ACRO_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Auto mode - allows vehicle to trace waypoints and perform automated actions
+#ifndef MODE_AUTO_ENABLED
+# define MODE_AUTO_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Brake mode - bring vehicle to stop
+#ifndef MODE_BRAKE_ENABLED
+# define MODE_BRAKE_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Circle - fly vehicle around a central point
+#ifndef MODE_CIRCLE_ENABLED
+# define MODE_CIRCLE_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Drift - fly vehicle in altitude-held, coordinated-turn mode
+#ifndef MODE_DRIFT_ENABLED
+# define MODE_DRIFT_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// flip - fly vehicle in flip in pitch and roll direction mode
+#ifndef MODE_FLIP_ENABLED
+# define MODE_FLIP_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Follow - follow another vehicle or GCS
+#ifndef MODE_FOLLOW_ENABLED
+# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Guided mode - control vehicle's position or angles from GCS
+#ifndef MODE_GUIDED_ENABLED
+# define MODE_GUIDED_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// GuidedNoGPS mode - control vehicle's angles from GCS
+#ifndef MODE_GUIDED_NOGPS_ENABLED
+# define MODE_GUIDED_NOGPS_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Loiter mode - allows vehicle to hold global position
+#ifndef MODE_LOITER_ENABLED
+# define MODE_LOITER_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Position Hold - enable holding of global position
+#ifndef MODE_POSHOLD_ENABLED
+# define MODE_POSHOLD_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// RTL - Return To Launch
+#ifndef MODE_RTL_ENABLED
+# define MODE_RTL_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home
+#ifndef MODE_SMARTRTL_ENABLED
+# define MODE_SMARTRTL_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Sport - fly vehicle in rate-controlled (earth-frame) mode
+#ifndef MODE_SPORT_ENABLED
+# define MODE_SPORT_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// System ID - conduct system identification tests on vehicle
+#ifndef MODE_SYSTEMID_ENABLED
+# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Button - Enable the button connected to AUX1-6
+#ifndef BUTTON_ENABLED
+# define BUTTON_ENABLED ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// RADIO CONFIGURATION
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+
+
+//////////////////////////////////////////////////////////////////////////////
+// FLIGHT_MODE
+//
+
+#ifndef FLIGHT_MODE_1
+# define FLIGHT_MODE_1                  Mode::Number::MANUAL
+#endif
+#ifndef FLIGHT_MODE_2
+# define FLIGHT_MODE_2                  Mode::Number::MANUAL
+#endif
+#ifndef FLIGHT_MODE_3
+# define FLIGHT_MODE_3                  Mode::Number::MANUAL
+#endif
+#ifndef FLIGHT_MODE_4
+# define FLIGHT_MODE_4                  Mode::Number::MANUAL
+#endif
+#ifndef FLIGHT_MODE_5
+# define FLIGHT_MODE_5                  Mode::Number::MANUAL
+#endif
+#ifndef FLIGHT_MODE_6
+# define FLIGHT_MODE_6                  Mode::Number::MANUAL
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Throttle Failsafe
+//
+#ifndef FS_THR_VALUE_DEFAULT
+# define FS_THR_VALUE_DEFAULT             975
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Takeoff
+//
+#ifndef PILOT_TKOFF_ALT_DEFAULT
+# define PILOT_TKOFF_ALT_DEFAULT           0     // default final alt above home for pilot initiated takeoff
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Landing
+//
+#ifndef LAND_SPEED
+# define LAND_SPEED    50          // the descent speed for the final stage of landing in cm/s
+#endif
+#ifndef LAND_REPOSITION_DEFAULT
+# define LAND_REPOSITION_DEFAULT   1   // by default the pilot can override roll/pitch during landing
+#endif
+#ifndef LAND_WITH_DELAY_MS
+# define LAND_WITH_DELAY_MS        4000    // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event
+#endif
+#ifndef LAND_CANCEL_TRIGGER_THR
+# define LAND_CANCEL_TRIGGER_THR   700     // land is cancelled by input throttle above 700
+#endif
+#ifndef LAND_RANGEFINDER_MIN_ALT_CM
+#define LAND_RANGEFINDER_MIN_ALT_CM 200
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Landing Detector
+//
+#ifndef LAND_DETECTOR_TRIGGER_SEC
+# define LAND_DETECTOR_TRIGGER_SEC         1.0f    // number of seconds to detect a landing
+#endif
+#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC
+# define LAND_DETECTOR_MAYBE_TRIGGER_SEC   0.2f    // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over)
+#endif
+#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF
+# define LAND_DETECTOR_ACCEL_LPF_CUTOFF     1.0f    // frequency cutoff of land detector accelerometer filter
+#endif
+#ifndef LAND_DETECTOR_ACCEL_MAX
+# define LAND_DETECTOR_ACCEL_MAX            1.0f    // vehicle acceleration must be under 1m/s/s
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// CAMERA TRIGGER AND CONTROL
+//
+#ifndef CAMERA
+# define CAMERA        ENABLED
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Flight mode definitions
+//
+
+// Acro Mode
+#ifndef ACRO_RP_P
+# define ACRO_RP_P                 4.5f
+#endif
+
+#ifndef ACRO_YAW_P
+# define ACRO_YAW_P                4.5f
+#endif
+
+#ifndef ACRO_LEVEL_MAX_ANGLE
+# define ACRO_LEVEL_MAX_ANGLE      3000
+#endif
+
+#ifndef ACRO_BALANCE_ROLL
+#define ACRO_BALANCE_ROLL          1.0f
+#endif
+
+#ifndef ACRO_BALANCE_PITCH
+#define ACRO_BALANCE_PITCH         1.0f
+#endif
+
+#ifndef ACRO_RP_EXPO_DEFAULT
+#define ACRO_RP_EXPO_DEFAULT       0.3f
+#endif
+
+#ifndef ACRO_Y_EXPO_DEFAULT
+#define ACRO_Y_EXPO_DEFAULT        0.0f
+#endif
+
+#ifndef ACRO_THR_MID_DEFAULT
+#define ACRO_THR_MID_DEFAULT       0.0f
+#endif
+
+// RTL Mode
+#ifndef RTL_ALT_FINAL
+# define RTL_ALT_FINAL             0       // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
+#endif
+
+#ifndef RTL_ALT
+# define RTL_ALT                   1500    // default alt to return to home in cm, 0 = Maintain current altitude
+#endif
+
+#ifndef RTL_ALT_MIN
+# define RTL_ALT_MIN               200     // min height above ground for RTL (i.e 2m)
+#endif
+
+#ifndef RTL_CLIMB_MIN_DEFAULT
+# define RTL_CLIMB_MIN_DEFAULT     0       // vehicle will always climb this many cm as first stage of RTL
+#endif
+
+#ifndef RTL_ABS_MIN_CLIMB
+# define RTL_ABS_MIN_CLIMB         250     // absolute minimum initial climb
+#endif
+
+#ifndef RTL_CONE_SLOPE_DEFAULT
+# define RTL_CONE_SLOPE_DEFAULT    3.0f    // slope of RTL cone (height / distance). 0 = No cone
+#endif
+
+#ifndef RTL_MIN_CONE_SLOPE
+# define RTL_MIN_CONE_SLOPE        0.5f    // minimum slope of cone
+#endif
+
+#ifndef RTL_LOITER_TIME
+# define RTL_LOITER_TIME           5000    // Time (in milliseconds) to loiter above home before beginning final descent
+#endif
+
+// AUTO Mode
+#ifndef WP_YAW_BEHAVIOR_DEFAULT
+# define WP_YAW_BEHAVIOR_DEFAULT   WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL
+#endif
+
+#ifndef AUTO_YAW_SLEW_RATE
+# define AUTO_YAW_SLEW_RATE    60              // degrees/sec
+#endif
+
+#ifndef YAW_LOOK_AHEAD_MIN_SPEED
+# define YAW_LOOK_AHEAD_MIN_SPEED  100             // minimum ground speed in cm/s required before blimp is aimed at ground course
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Stabilize Rate Control
+//
+#ifndef ROLL_PITCH_YAW_INPUT_MAX
+# define ROLL_PITCH_YAW_INPUT_MAX      4500        // roll, pitch and yaw input range
+#endif
+#ifndef DEFAULT_ANGLE_MAX
+# define DEFAULT_ANGLE_MAX         4500            // ANGLE_MAX parameters default value
+#endif
+#ifndef ANGLE_RATE_MAX
+# define ANGLE_RATE_MAX            18000           // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Stop mode defaults
+//
+#ifndef BRAKE_MODE_SPEED_Z
+# define BRAKE_MODE_SPEED_Z     250 // z-axis speed in cm/s in Brake Mode
+#endif
+#ifndef BRAKE_MODE_DECEL_RATE
+# define BRAKE_MODE_DECEL_RATE  750 // acceleration rate in cm/s/s in Brake Mode
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// PosHold parameter defaults
+//
+#ifndef POSHOLD_BRAKE_RATE_DEFAULT
+# define POSHOLD_BRAKE_RATE_DEFAULT    8       // default POSHOLD_BRAKE_RATE param value.  Rotation rate during braking in deg/sec
+#endif
+#ifndef POSHOLD_BRAKE_ANGLE_DEFAULT
+# define POSHOLD_BRAKE_ANGLE_DEFAULT   3000    // default POSHOLD_BRAKE_ANGLE param value.  Max lean angle during braking in centi-degrees
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Throttle control defaults
+//
+
+#ifndef THR_DZ_DEFAULT
+# define THR_DZ_DEFAULT         100             // the deadzone above and below mid throttle while in althold or loiter
+#endif
+
+// default maximum vertical velocity and acceleration the pilot may request
+#ifndef PILOT_VELZ_MAX
+# define PILOT_VELZ_MAX    250     // maximum vertical velocity in cm/s
+#endif
+#ifndef PILOT_ACCEL_Z_DEFAULT
+# define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control
+#endif
+
+// max distance in cm above or below current location that will be used for the alt target when transitioning to alt-hold mode
+#ifndef ALT_HOLD_INIT_MAX_OVERSHOOT
+# define ALT_HOLD_INIT_MAX_OVERSHOOT 200
+#endif
+// the acceleration used to define the distance-velocity curve
+#ifndef ALT_HOLD_ACCEL_MAX
+# define ALT_HOLD_ACCEL_MAX 250    // if you change this you must also update the duplicate declaration in AC_WPNav.h
+#endif
+
+#ifndef AUTO_DISARMING_DELAY
+# define AUTO_DISARMING_DELAY  10
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Throw mode configuration
+//
+#ifndef THROW_HIGH_SPEED
+# define THROW_HIGH_SPEED       500.0f  // vehicle much reach this total 3D speed in cm/s (or be free falling)
+#endif
+#ifndef THROW_VERTICAL_SPEED
+# define THROW_VERTICAL_SPEED   50.0f   // motors start when vehicle reaches this total 3D speed in cm/s
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Logging control
+//
+#ifndef LOGGING_ENABLED
+# define LOGGING_ENABLED                ENABLED
+#endif
+
+// Default logging bitmask
+#ifndef DEFAULT_LOG_BITMASK
+# define DEFAULT_LOG_BITMASK \
+    MASK_LOG_ATTITUDE_MED | \
+    MASK_LOG_GPS | \
+    MASK_LOG_PM | \
+    MASK_LOG_CTUN | \
+    MASK_LOG_NTUN | \
+    MASK_LOG_RCIN | \
+    MASK_LOG_IMU | \
+    MASK_LOG_CMD | \
+    MASK_LOG_CURRENT | \
+    MASK_LOG_RCOUT | \
+    MASK_LOG_OPTFLOW | \
+    MASK_LOG_COMPASS | \
+    MASK_LOG_CAMERA | \
+    MASK_LOG_MOTBATT
+#endif
+
+#ifndef CH_MODE_DEFAULT
+# define CH_MODE_DEFAULT   5
+#endif
+
+#ifndef STATS_ENABLED
+# define STATS_ENABLED ENABLED
+#endif
+
+#ifndef HAL_FRAME_TYPE_DEFAULT
+#define HAL_FRAME_TYPE_DEFAULT Fins::MOTOR_FRAME_TYPE_AIRFISH
+#endif

Blimp/defines.h

@@ -0,0 +1,209 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+// Just so that it's completely clear...
+#define ENABLED                 1
+#define DISABLED                0
+
+// this avoids a very common config error
+#define ENABLE ENABLED
+#define DISABLE DISABLED
+
+// Autopilot Yaw Mode enumeration
+enum autopilot_yaw_mode {
+    AUTO_YAW_HOLD =             0,  // pilot controls the heading
+    AUTO_YAW_LOOK_AT_NEXT_WP =  1,  // point towards next waypoint (no pilot input accepted)
+    AUTO_YAW_ROI =              2,  // point towards a location held in roi (no pilot input accepted)
+    AUTO_YAW_FIXED =            3,  // point towards a particular angle (no pilot input accepted)
+    AUTO_YAW_LOOK_AHEAD =       4,  // point in the direction the blimp is moving
+    AUTO_YAW_RESETTOARMEDYAW =  5,  // point towards heading at time motors were armed
+    AUTO_YAW_RATE =             6,  // turn at a specified rate (held in auto_yaw_rate)
+    AUTO_YAW_CIRCLE =           7,  // use AC_Circle's provided yaw (used during Loiter-Turns commands)
+};
+
+// Frame types
+#define UNDEFINED_FRAME 0
+#define MULTICOPTER_FRAME 1
+#define HELI_FRAME 2
+
+// HIL enumerations
+#define HIL_MODE_DISABLED               0
+#define HIL_MODE_SENSORS                1
+
+// Tuning enumeration
+enum tuning_func {
+    TUNING_NONE =                        0, //
+    TUNING_STABILIZE_ROLL_PITCH_KP =     1, // stabilize roll/pitch angle controller's P term
+    TUNING_STABILIZE_YAW_KP =            3, // stabilize yaw heading controller's P term
+    TUNING_RATE_ROLL_PITCH_KP =          4, // body frame roll/pitch rate controller's P term
+    TUNING_RATE_ROLL_PITCH_KI =          5, // body frame roll/pitch rate controller's I term
+    TUNING_YAW_RATE_KP =                 6, // body frame yaw rate controller's P term
+    TUNING_THROTTLE_RATE_KP =            7, // throttle rate controller's P term (desired rate to acceleration or motor output)
+    TUNING_WP_SPEED =                   10, // maximum speed to next way point (0 to 10m/s)
+    TUNING_LOITER_POSITION_KP =         12, // loiter distance controller's P term (position error to speed)
+    TUNING_HELI_EXTERNAL_GYRO =         13, // TradHeli specific external tail gyro gain
+    TUNING_ALTITUDE_HOLD_KP =           14, // altitude hold controller's P term (alt error to desired rate)
+    TUNING_RATE_ROLL_PITCH_KD =         21, // body frame roll/pitch rate controller's D term
+    TUNING_VEL_XY_KP =                  22, // loiter rate controller's P term (speed error to tilt angle)
+    TUNING_ACRO_RP_KP =                 25, // acro controller's P term.  converts pilot input to a desired roll, pitch or yaw rate
+    TUNING_YAW_RATE_KD =                26, // body frame yaw rate controller's D term
+    TUNING_VEL_XY_KI =                  28, // loiter rate controller's I term (speed error to tilt angle)
+    TUNING_AHRS_YAW_KP =                30, // ahrs's compass effect on yaw angle (0 = very low, 1 = very high)
+    TUNING_AHRS_KP =                    31, // accelerometer effect on roll/pitch angle (0=low)
+    TUNING_ACCEL_Z_KP =                 34, // accel based throttle controller's P term
+    TUNING_ACCEL_Z_KI =                 35, // accel based throttle controller's I term
+    TUNING_ACCEL_Z_KD =                 36, // accel based throttle controller's D term
+    TUNING_DECLINATION =                38, // compass declination in radians
+    TUNING_CIRCLE_RATE =                39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
+    TUNING_ACRO_YAW_KP =                40, // acro controller's P term.  converts pilot input to a desired roll, pitch or yaw rate
+    TUNING_RANGEFINDER_GAIN =           41, // rangefinder gain
+    TUNING_EKF_VERTICAL_POS =           42, // unused
+    TUNING_EKF_HORIZONTAL_POS =         43, // unused
+    TUNING_EKF_ACCEL_NOISE =            44, // unused
+    TUNING_RC_FEEL_RP =                 45, // roll-pitch input smoothing
+    TUNING_RATE_PITCH_KP =              46, // body frame pitch rate controller's P term
+    TUNING_RATE_PITCH_KI =              47, // body frame pitch rate controller's I term
+    TUNING_RATE_PITCH_KD =              48, // body frame pitch rate controller's D term
+    TUNING_RATE_ROLL_KP =               49, // body frame roll rate controller's P term
+    TUNING_RATE_ROLL_KI =               50, // body frame roll rate controller's I term
+    TUNING_RATE_ROLL_KD =               51, // body frame roll rate controller's D term
+    TUNING_RATE_PITCH_FF =              52, // body frame pitch rate controller FF term
+    TUNING_RATE_ROLL_FF =               53, // body frame roll rate controller FF term
+    TUNING_RATE_YAW_FF =                54, // body frame yaw rate controller FF term
+    TUNING_RATE_MOT_YAW_HEADROOM =      55, // motors yaw headroom minimum
+    TUNING_RATE_YAW_FILT =              56, // yaw rate input filter
+    UNUSED =                            57, // was winch control
+    TUNING_SYSTEM_ID_MAGNITUDE =        58  // magnitude of the system ID signal
+};
+
+// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter
+#define WP_YAW_BEHAVIOR_NONE                          0   // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received)
+#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP               1   // auto pilot will face next waypoint or home during rtl
+#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL    2   // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
+#define WP_YAW_BEHAVIOR_LOOK_AHEAD                    3   // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicopters)
+
+// Auto modes
+enum AutoMode {
+    Auto_TakeOff,
+    Auto_WP,
+    Auto_Land,
+    Auto_RTL,
+    Auto_CircleMoveToEdge,
+    Auto_Circle,
+    Auto_Spline,
+    Auto_NavGuided,
+    Auto_Loiter,
+    Auto_LoiterToAlt,
+    Auto_NavPayloadPlace,
+};
+
+// Guided modes
+enum GuidedMode {
+    Guided_TakeOff,
+    Guided_WP,
+    Guided_Velocity,
+    Guided_PosVel,
+    Guided_Angle,
+};
+
+// // Airmode
+// enum class AirMode {
+//     AIRMODE_NONE,
+//     AIRMODE_DISABLED,
+//     AIRMODE_ENABLED,
+// };
+
+// // Safe RTL states
+// enum SmartRTLState {
+//     SmartRTL_WaitForPathCleanup,
+//     SmartRTL_PathFollow,
+//     SmartRTL_PreLandPosition,
+//     SmartRTL_Descend,
+//     SmartRTL_Land
+// };
+
+enum PayloadPlaceStateType {
+    PayloadPlaceStateType_FlyToLocation,
+    PayloadPlaceStateType_Calibrating_Hover_Start,
+    PayloadPlaceStateType_Calibrating_Hover,
+    PayloadPlaceStateType_Descending_Start,
+    PayloadPlaceStateType_Descending,
+    PayloadPlaceStateType_Releasing_Start,
+    PayloadPlaceStateType_Releasing,
+    PayloadPlaceStateType_Released,
+    PayloadPlaceStateType_Ascending_Start,
+    PayloadPlaceStateType_Ascending,
+    PayloadPlaceStateType_Done,
+};
+
+// bit options for DEV_OPTIONS parameter
+enum DevOptions {
+    DevOptionADSBMAVLink = 1,
+    DevOptionVFR_HUDRelativeAlt = 2,
+    DevOptionSetAttitudeTarget_ThrustAsThrust = 4,
+};
+
+//  Logging parameters
+enum LoggingParameters {
+    LOG_CONTROL_TUNING_MSG,
+    LOG_DATA_INT16_MSG,
+    LOG_DATA_UINT16_MSG,
+    LOG_DATA_INT32_MSG,
+    LOG_DATA_UINT32_MSG,
+    LOG_DATA_FLOAT_MSG,
+    LOG_MOTBATT_MSG,
+    LOG_PARAMTUNE_MSG,
+    LOG_HELI_MSG,
+    LOG_PRECLAND_MSG,
+    LOG_GUIDEDTARGET_MSG,
+    LOG_SYSIDD_MSG,
+    LOG_SYSIDS_MSG,
+};
+
+#define MASK_LOG_ATTITUDE_FAST          (1<<0)
+#define MASK_LOG_ATTITUDE_MED           (1<<1)
+#define MASK_LOG_GPS                    (1<<2)
+#define MASK_LOG_PM                     (1<<3)
+#define MASK_LOG_CTUN                   (1<<4)
+#define MASK_LOG_NTUN                   (1<<5)
+#define MASK_LOG_RCIN                   (1<<6)
+#define MASK_LOG_IMU                    (1<<7)
+#define MASK_LOG_CMD                    (1<<8)
+#define MASK_LOG_CURRENT                (1<<9)
+#define MASK_LOG_RCOUT                  (1<<10)
+#define MASK_LOG_OPTFLOW                (1<<11)
+#define MASK_LOG_PID                    (1<<12)
+#define MASK_LOG_COMPASS                (1<<13)
+#define MASK_LOG_INAV                   (1<<14) // deprecated
+#define MASK_LOG_CAMERA                 (1<<15)
+#define MASK_LOG_MOTBATT                (1UL<<17)
+#define MASK_LOG_IMU_FAST               (1UL<<18)
+#define MASK_LOG_IMU_RAW                (1UL<<19)
+#define MASK_LOG_ANY                    0xFFFF
+
+// Radio failsafe definitions (FS_THR parameter)
+#define FS_THR_DISABLED                            0
+#define FS_THR_ENABLED_ALWAYS_RTL                  1
+#define FS_THR_ENABLED_CONTINUE_MISSION            2    // Removed in 4.0+, now use fs_options
+#define FS_THR_ENABLED_ALWAYS_LAND                 3
+#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL      4
+#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND     5
+
+// GCS failsafe definitions (FS_GCS_ENABLE parameter)
+#define FS_GCS_DISABLED                        0
+#define FS_GCS_ENABLED_ALWAYS_RTL              1
+#define FS_GCS_ENABLED_CONTINUE_MISSION        2    // Removed in 4.0+, now use fs_options
+#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL  3
+#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4
+#define FS_GCS_ENABLED_ALWAYS_LAND             5
+
+// EKF failsafe definitions (FS_EKF_ACTION parameter)
+#define FS_EKF_ACTION_LAND                  1       // switch to LAND mode on EKF failsafe
+#define FS_EKF_ACTION_ALTHOLD               2       // switch to ALTHOLD mode on EKF failsafe
+#define FS_EKF_ACTION_LAND_EVEN_STABILIZE   3       // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize
+
+// for PILOT_THR_BHV parameter
+#define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0)
+#define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1)
+#define THR_BEHAVE_DISARM_ON_LAND_DETECT (1<<2)

Blimp/ekf_check.cpp

@@ -0,0 +1,272 @@
+#include "Blimp.h"
+
+/**
+ *
+ * Detects failures of the ekf or inertial nav system triggers an alert
+ * to the pilot and helps take countermeasures
+ *
+ */
+
+#ifndef EKF_CHECK_ITERATIONS_MAX
+# define EKF_CHECK_ITERATIONS_MAX          10      // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
+#endif
+
+#ifndef EKF_CHECK_WARNING_TIME
+# define EKF_CHECK_WARNING_TIME            (30*1000)   // warning text messages are sent to ground no more than every 30 seconds
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// EKF_check structure
+////////////////////////////////////////////////////////////////////////////////
+static struct {
+    uint8_t fail_count;         // number of iterations ekf or dcm have been out of tolerances
+    uint8_t bad_variance : 1;   // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
+    uint32_t last_warn_time;    // system time of last warning in milliseconds.  Used to throttle text warnings sent to GCS
+} ekf_check_state;
+
+// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
+// should be called at 10hz
+void Blimp::ekf_check()
+{
+    // ensure EKF_CHECK_ITERATIONS_MAX is at least 7
+    static_assert(EKF_CHECK_ITERATIONS_MAX >= 7, "EKF_CHECK_ITERATIONS_MAX must be at least 7");
+
+    // exit immediately if ekf has no origin yet - this assumes the origin can never become unset
+    Location temp_loc;
+    if (!ahrs.get_origin(temp_loc)) {
+        return;
+    }
+
+    // return immediately if motors are not armed, or ekf check is disabled
+    if (!motors->armed() || (g.fs_ekf_thresh <= 0.0f)) {
+        ekf_check_state.fail_count = 0;
+        ekf_check_state.bad_variance = false;
+        AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
+        failsafe_ekf_off_event();   // clear failsafe
+        return;
+    }
+
+    // compare compass and velocity variance vs threshold and also check
+    // if we are still navigating
+    bool is_navigating = ekf_has_relative_position() || ekf_has_absolute_position();
+    if (ekf_over_threshold() || !is_navigating) {
+        // if compass is not yet flagged as bad
+        if (!ekf_check_state.bad_variance) {
+            // increase counter
+            ekf_check_state.fail_count++;
+            if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2) && ekf_over_threshold()) {
+                // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
+                // yaw to resolve the issue
+                ahrs.request_yaw_reset();
+            }
+            if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-1)) {
+                // we are just about to declare a EKF failsafe, ask the EKF if we can
+                // change lanes to resolve the issue
+                ahrs.check_lane_switch();
+            }
+            // if counter above max then trigger failsafe
+            if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
+                // limit count from climbing too high
+                ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
+                ekf_check_state.bad_variance = true;
+                AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
+                // send message to gcs
+                if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
+                    gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
+                    ekf_check_state.last_warn_time = AP_HAL::millis();
+                }
+                failsafe_ekf_event();
+            }
+        }
+    } else {
+        // reduce counter
+        if (ekf_check_state.fail_count > 0) {
+            ekf_check_state.fail_count--;
+
+            // if compass is flagged as bad and the counter reaches zero then clear flag
+            if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
+                ekf_check_state.bad_variance = false;
+                AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
+                // clear failsafe
+                failsafe_ekf_off_event();
+            }
+        }
+    }
+
+    // set AP_Notify flags
+    AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
+
+    // To-Do: add ekf variances to extended status
+}
+
+// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
+bool Blimp::ekf_over_threshold()
+{
+    // return false immediately if disabled
+    if (g.fs_ekf_thresh <= 0.0f) {
+        return false;
+    }
+
+    // use EKF to get variance
+    float position_variance, vel_variance, height_variance, tas_variance;
+    Vector3f mag_variance;
+    ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance);
+
+    const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
+
+    // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold
+    uint8_t over_thresh_count = 0;
+    if (mag_max >= g.fs_ekf_thresh) {
+        over_thresh_count++;
+    }
+
+    bool optflow_healthy = false;
+    if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) {
+        over_thresh_count += 2;
+    } else if (vel_variance >= g.fs_ekf_thresh) {
+        over_thresh_count++;
+    }
+
+    if ((position_variance >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
+        return true;
+    }
+
+    return false;
+}
+
+
+// failsafe_ekf_event - perform ekf failsafe
+void Blimp::failsafe_ekf_event()
+{
+    // return immediately if ekf failsafe already triggered
+    if (failsafe.ekf) {
+        return;
+    }
+
+    // EKF failsafe event has occurred
+    failsafe.ekf = true;
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
+
+    // // sometimes LAND *does* require GPS so ensure we are in non-GPS land
+    // if (control_mode == Mode::Number::LAND && landing_with_GPS()) {
+    //     mode_land.do_not_use_GPS();
+    //     return;
+    // }
+    // LAND never requires GPS.
+
+    // does this mode require position?
+    if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
+        return;
+    }
+
+    // take action based on fs_ekf_action parameter
+    switch (g.fs_ekf_action) {
+    case FS_EKF_ACTION_LAND:
+    case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
+    default:
+        set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
+        break;
+    }
+}
+
+// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
+void Blimp::failsafe_ekf_off_event(void)
+{
+    // return immediately if not in ekf failsafe
+    if (!failsafe.ekf) {
+        return;
+    }
+
+    failsafe.ekf = false;
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
+}
+
+// check for ekf yaw reset and adjust target heading, also log position reset
+void Blimp::check_ekf_reset()
+{
+    // check for yaw reset
+    float yaw_angle_change_rad;
+    uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
+    if (new_ekfYawReset_ms != ekfYawReset_ms) {
+        // attitude_control->inertial_frame_reset();
+        ekfYawReset_ms = new_ekfYawReset_ms;
+        AP::logger().Write_Event(LogEvent::EKF_YAW_RESET);
+    }
+
+#if AP_AHRS_NAVEKF_AVAILABLE && (HAL_NAVEKF2_AVAILABLE || HAL_NAVEKF3_AVAILABLE)
+    // check for change in primary EKF, reset attitude target and log.  AC_PosControl handles position target adjustment
+    if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) {
+        // attitude_control->inertial_frame_reset();
+        ekf_primary_core = ahrs.get_primary_core_index();
+        AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core));
+        gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core);
+    }
+#endif
+}
+
+// check for high vibrations affecting altitude control
+void Blimp::check_vibration()
+{
+    uint32_t now = AP_HAL::millis();
+
+    // assume checks will succeed
+    bool checks_succeeded = true;
+
+    // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
+    Vector3f vel_innovation;
+    Vector3f pos_innovation;
+    Vector3f mag_innovation;
+    float tas_innovation;
+    float yaw_innovation;
+    if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
+        checks_succeeded = false;
+    }
+    const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
+
+    // check if vertical velocity variance is at least 1 (NK4.SV >= 1.0)
+    float position_variance, vel_variance, height_variance, tas_variance;
+    Vector3f mag_variance;
+    if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) {
+        checks_succeeded = false;
+    }
+
+    // if no failure
+    if ((g2.fs_vibe_enabled == 0) || !checks_succeeded || !motors->armed() || !innov_velD_posD_positive || (vel_variance < 1.0f)) {
+        if (vibration_check.high_vibes) {
+            // start clear time
+            if (vibration_check.clear_ms == 0) {
+                vibration_check.clear_ms = now;
+                return;
+            }
+            // turn off vibration compensation after 15 seconds
+            if (now - vibration_check.clear_ms > 15000) {
+                // restore ekf gains, reset timers and update user
+                vibration_check.high_vibes = false;
+                // pos_control->set_vibe_comp(false);
+                vibration_check.clear_ms = 0;
+                AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
+                gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
+            }
+        }
+        vibration_check.start_ms = 0;
+        return;
+    }
+
+    // start timer
+    if (vibration_check.start_ms == 0) {
+        vibration_check.start_ms = now;
+        vibration_check.clear_ms = 0;
+        return;
+    }
+
+    // check if failure has persisted for at least 1 second
+    if (now - vibration_check.start_ms > 1000) {
+        if (!vibration_check.high_vibes) {
+            // switch ekf to use resistant gains
+            vibration_check.high_vibes = true;
+            // pos_control->set_vibe_comp(true);
+            AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
+        }
+    }
+}

Blimp/events.cpp

@@ -0,0 +1,343 @@
+#include "Blimp.h"
+
+/*
+ *       This event will be called when the failsafe changes
+ *       boolean failsafe reflects the current state
+ */
+
+
+bool Blimp::failsafe_option(FailsafeOption opt) const
+{
+    return (g2.fs_options & (uint32_t)opt);
+}
+
+void Blimp::failsafe_radio_on_event()
+{
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED);
+
+    // set desired action based on FS_THR_ENABLE parameter
+    Failsafe_Action desired_action;
+    switch (g.failsafe_throttle) {
+    case FS_THR_DISABLED:
+        desired_action = Failsafe_Action_None;
+        break;
+    case FS_THR_ENABLED_ALWAYS_LAND:
+        desired_action = Failsafe_Action_Land;
+        break;
+    default:
+        desired_action = Failsafe_Action_Land;
+    }
+
+    // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning
+    if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
+        arming.disarm(AP_Arming::Method::RADIOFAILSAFE);
+        desired_action = Failsafe_Action_None;
+
+    } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
+        // Allow landing to continue when battery failsafe requires it (not a user option)
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
+        // Allow landing to continue when FS_OPTIONS is set to continue landing
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe");
+    }
+
+    // Call the failsafe action handler
+    do_failsafe_action(desired_action, ModeReason::RADIO_FAILSAFE);
+}
+
+// failsafe_off_event - respond to radio contact being regained
+void Blimp::failsafe_radio_off_event()
+{
+    // no need to do anything except log the error as resolved
+    // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED);
+    gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared");
+}
+
+void Blimp::handle_battery_failsafe(const char *type_str, const int8_t action)
+{
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
+
+    Failsafe_Action desired_action = (Failsafe_Action)action;
+
+    // Conditions to deviate from BATT_FS_XXX_ACT parameter setting
+    if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
+        arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming");
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != Failsafe_Action_None) {
+        // Allow landing to continue when FS_OPTIONS is set to continue when landing
+        desired_action = Failsafe_Action_Land;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
+    }
+
+    // Battery FS options already use the Failsafe_Options enum. So use them directly.
+    do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE);
+
+}
+// failsafe_gcs_check - check for ground station failsafe
+void Blimp::failsafe_gcs_check()
+{
+    // Bypass GCS failsafe checks if disabled or GCS never connected
+    if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) {
+        return;
+    }
+
+    // calc time since last gcs update
+    // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
+    const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
+    const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
+
+    // Determine which event to trigger
+    if (last_gcs_update_ms < gcs_timeout_ms && failsafe.gcs) {
+        // Recovery from a GCS failsafe
+        set_failsafe_gcs(false);
+        // failsafe_gcs_off_event();
+
+    } else if (last_gcs_update_ms < gcs_timeout_ms && !failsafe.gcs) {
+        // No problem, do nothing
+
+    } else if (last_gcs_update_ms > gcs_timeout_ms && failsafe.gcs) {
+        // Already in failsafe, do nothing
+
+    } else if (last_gcs_update_ms > gcs_timeout_ms && !failsafe.gcs) {
+        // New GCS failsafe event, trigger events
+        set_failsafe_gcs(true);
+        // failsafe_gcs_on_event();
+    }
+}
+/*
+// failsafe_gcs_on_event - actions to take when GCS contact is lost
+void Blimp::failsafe_gcs_on_event(void)
+{
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
+    RC_Channels::clear_overrides();
+
+    // convert the desired failsafe response to the Failsafe_Action enum
+    Failsafe_Action desired_action;
+    switch (g.failsafe_gcs) {
+        case FS_GCS_DISABLED:
+            desired_action = Failsafe_Action_None;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_RTL:
+        case FS_GCS_ENABLED_CONTINUE_MISSION:
+            desired_action = Failsafe_Action_RTL;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
+            desired_action = Failsafe_Action_SmartRTL;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
+            desired_action = Failsafe_Action_SmartRTL_Land;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_LAND:
+            desired_action = Failsafe_Action_Land;
+            break;
+        default: // if an invalid parameter value is set, the fallback is RTL
+            desired_action = Failsafe_Action_RTL;
+    }
+
+    // Conditions to deviate from FS_GCS_ENABLE parameter setting
+    if (!motors->armed()) {
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
+
+    } else if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
+        arming.disarm(AP_Arming::Method::GCSFAILSAFE);
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming");
+
+    } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
+        // Allow landing to continue when battery failsafe requires it (not a user option)
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
+        // Allow landing to continue when FS_OPTIONS is set to continue landing
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) {
+        // Allow mission to continue when FS_OPTIONS is set to continue mission
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode");
+        desired_action = Failsafe_Action_None;
+
+    } else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) {
+        // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control");
+        desired_action = Failsafe_Action_None;
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
+    }
+
+    // Call the failsafe action handler
+    do_failsafe_action(desired_action, ModeReason::GCS_FAILSAFE);
+}
+
+// failsafe_gcs_off_event - actions to take when GCS contact is restored
+void Blimp::failsafe_gcs_off_event(void)
+{
+    gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared");
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
+}
+
+// executes terrain failsafe if data is missing for longer than a few seconds
+void Blimp::failsafe_terrain_check()
+{
+    // trigger within <n> milliseconds of failures while in various modes
+    bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
+    bool trigger_event = timeout && flightmode->requires_terrain_failsafe();
+
+    // check for clearing of event
+    if (trigger_event != failsafe.terrain) {
+        if (trigger_event) {
+            failsafe_terrain_on_event();
+        } else {
+            AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED);
+            failsafe.terrain = false;
+        }
+    }
+}
+
+// set terrain data status (found or not found)
+void Blimp::failsafe_terrain_set_status(bool data_ok)
+{
+    uint32_t now = millis();
+
+    // record time of first and latest failures (i.e. duration of failures)
+    if (!data_ok) {
+        failsafe.terrain_last_failure_ms = now;
+        if (failsafe.terrain_first_failure_ms == 0) {
+            failsafe.terrain_first_failure_ms = now;
+        }
+    } else {
+        // failures cleared after 0.1 seconds of persistent successes
+        if (now - failsafe.terrain_last_failure_ms > 100) {
+            failsafe.terrain_last_failure_ms = 0;
+            failsafe.terrain_first_failure_ms = 0;
+        }
+    }
+}
+
+// terrain failsafe action
+void Blimp::failsafe_terrain_on_event()
+{
+    failsafe.terrain = true;
+    gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing");
+    AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
+
+    if (should_disarm_on_failsafe()) {
+        arming.disarm(AP_Arming::Method::TERRAINFAILSAFE);
+#if MODE_RTL_ENABLED == ENABLED
+    } else if (control_mode == Mode::Number::RTL) {
+        mode_rtl.restart_without_terrain();
+#endif
+    } else {
+        set_mode_RTL_or_land_with_pause(ModeReason::TERRAIN_FAILSAFE);
+    }
+}
+
+// check for gps glitch failsafe
+void Blimp::gpsglitch_check()
+{
+    // get filter status
+    nav_filter_status filt_status = inertial_nav.get_filter_status();
+    bool gps_glitching = filt_status.flags.gps_glitching;
+
+    // log start or stop of gps glitch.  AP_Notify update is handled from within AP_AHRS
+    if (ap.gps_glitching != gps_glitching) {
+        ap.gps_glitching = gps_glitching;
+        if (gps_glitching) {
+            AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH);
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch");
+        } else {
+            AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED);
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared");
+        }
+    }
+}
+
+// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
+//  this is always called from a failsafe so we trigger notification to pilot
+void Blimp::set_mode_RTL_or_land_with_pause(ModeReason reason)
+{
+    // attempt to switch to RTL, if this fails then switch to Land
+    if (!set_mode(Mode::Number::RTL, reason)) {
+        // set mode to land will trigger mode change notification to pilot
+        set_mode_land_with_pause(reason);
+    } else {
+        // alert pilot to mode change
+        AP_Notify::events.failsafe_mode_change = 1;
+    }
+}
+
+// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts
+// this is always called from a failsafe so we trigger notification to pilot
+void Blimp::set_mode_SmartRTL_or_land_with_pause(ModeReason reason)
+{
+    // attempt to switch to SMART_RTL, if this failed then switch to Land
+    if (!set_mode(Mode::Number::SMART_RTL, reason)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
+        set_mode_land_with_pause(reason);
+    } else {
+        AP_Notify::events.failsafe_mode_change = 1;
+    }
+}
+
+// set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts
+// this is always called from a failsafe so we trigger notification to pilot
+void Blimp::set_mode_SmartRTL_or_RTL(ModeReason reason)
+{
+    // attempt to switch to SmartRTL, if this failed then attempt to RTL
+    // if that fails, then land
+    if (!set_mode(Mode::Number::SMART_RTL, reason)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
+        set_mode_RTL_or_land_with_pause(reason);
+    } else {
+        AP_Notify::events.failsafe_mode_change = 1;
+    }
+}
+*/
+bool Blimp::should_disarm_on_failsafe()
+{
+    if (ap.in_arming_delay) {
+        return true;
+    }
+
+    switch (control_mode) {
+    case Mode::Number::MANUAL:
+    default:
+        // if landed disarm
+        return ap.land_complete;
+    }
+}
+
+
+void Blimp::do_failsafe_action(Failsafe_Action action, ModeReason reason)
+{
+
+    // Execute the specified desired_action
+    switch (action) {
+    case Failsafe_Action_None:
+        return;
+    case Failsafe_Action_Land:
+        set_mode_land_with_pause(reason);
+        break;
+    case Failsafe_Action_Terminate: {
+        arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
+    }
+    break;
+    }
+}

Blimp/failsafe.cpp

@@ -0,0 +1,72 @@
+#include "Blimp.h"
+
+//
+//  failsafe support
+//  Andrew Tridgell, December 2011
+//
+//  our failsafe strategy is to detect main loop lockup and disarm the motors
+//
+
+static bool failsafe_enabled = false;
+static uint16_t failsafe_last_ticks;
+static uint32_t failsafe_last_timestamp;
+static bool in_failsafe;
+
+//
+// failsafe_enable - enable failsafe
+//
+void Blimp::failsafe_enable()
+{
+    failsafe_enabled = true;
+    failsafe_last_timestamp = micros();
+}
+
+//
+// failsafe_disable - used when we know we are going to delay the mainloop significantly
+//
+void Blimp::failsafe_disable()
+{
+    failsafe_enabled = false;
+}
+
+//
+//  failsafe_check - this function is called from the core timer interrupt at 1kHz.
+//
+void Blimp::failsafe_check()
+{
+    uint32_t tnow = AP_HAL::micros();
+
+    const uint16_t ticks = scheduler.ticks();
+    if (ticks != failsafe_last_ticks) {
+        // the main loop is running, all is OK
+        failsafe_last_ticks = ticks;
+        failsafe_last_timestamp = tnow;
+        if (in_failsafe) {
+            in_failsafe = false;
+            AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED);
+        }
+        return;
+    }
+
+    if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
+        // motors are running but we have gone 2 second since the
+        // main loop ran. That means we're in trouble and should
+        // disarm the motors->
+        in_failsafe = true;
+        // reduce motors to minimum (we do not immediately disarm because we want to log the failure)
+        if (motors->armed()) {
+            motors->output_min();
+        }
+
+        AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED);
+    }
+
+    if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
+        // disarm motors every second
+        failsafe_last_timestamp = tnow;
+        if (motors->armed()) {
+            motors->armed(false);
+            motors->output();
+        }
+    }
+}

Blimp/inertia.cpp

@@ -0,0 +1,27 @@
+#include "Blimp.h"
+
+// read_inertia - read inertia in from accelerometers
+void Blimp::read_inertia()
+{
+    // inertial altitude estimates. Use barometer climb rate during high vibrations
+    inertial_nav.update(vibration_check.high_vibes);
+
+    // pull position from ahrs
+    Location loc;
+    ahrs.get_position(loc);
+    current_loc.lat = loc.lat;
+    current_loc.lng = loc.lng;
+
+    // exit immediately if we do not have an altitude estimate
+    if (!inertial_nav.get_filter_status().flags.vert_pos) {
+        return;
+    }
+
+    // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin
+    const int32_t alt_above_origin_cm = inertial_nav.get_altitude();
+    current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN);
+    if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
+        // if home has not been set yet we treat alt-above-origin as alt-above-home
+        current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_HOME);
+    }
+}

Blimp/mode.cpp

@@ -0,0 +1,537 @@
+#include "Blimp.h"
+
+/*
+ * High level calls to set and update flight modes logic for individual
+ * flight modes is in control_acro.cpp, control_stabilize.cpp, etc
+ */
+
+/*
+  constructor for Mode object
+ */
+Mode::Mode(void) :
+    g(blimp.g),
+    g2(blimp.g2),
+    // wp_nav(blimp.wp_nav),
+    // loiter_nav(blimp.loiter_nav),
+    // pos_control(blimp.pos_control),
+    inertial_nav(blimp.inertial_nav),
+    ahrs(blimp.ahrs),
+    // attitude_control(blimp.attitude_control),
+    motors(blimp.motors),
+    channel_right(blimp.channel_right),
+    channel_front(blimp.channel_front),
+    channel_down(blimp.channel_down),
+    channel_yaw(blimp.channel_yaw),
+    G_Dt(blimp.G_Dt)
+{ };
+
+// return the static controller object corresponding to supplied mode
+Mode *Blimp::mode_from_mode_num(const Mode::Number mode)
+{
+    Mode *ret = nullptr;
+
+    switch (mode) {
+    case Mode::Number::MANUAL:
+        ret = &mode_manual;
+        break;
+    case Mode::Number::LAND:
+        ret = &mode_land;
+        break;
+    default:
+        break;
+    }
+
+    return ret;
+}
+
+
+// set_mode - change flight mode and perform any necessary initialisation
+// optional force parameter used to force the flight mode change (used only first time mode is set)
+// returns true if mode was successfully set
+// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
+bool Blimp::set_mode(Mode::Number mode, ModeReason reason)
+{
+
+    // return immediately if we are already in the desired mode
+    if (mode == control_mode) {
+        control_mode_reason = reason;
+        return true;
+    }
+
+    Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode);
+    if (new_flightmode == nullptr) {
+        gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
+        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+        return false;
+    }
+
+    bool ignore_checks = !motors->armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform
+
+    // ensure vehicle doesn't leap off the ground if a user switches
+    // into a manual throttle mode from a non-manual-throttle mode
+    // (e.g. user arms in guided, raises throttle to 1300 (not enough to
+    // trigger auto takeoff), then switches into manual):
+    bool user_throttle = new_flightmode->has_manual_throttle();
+    if (!ignore_checks &&
+        ap.land_complete &&
+        user_throttle &&
+        !blimp.flightmode->has_manual_throttle() &&
+        new_flightmode->get_pilot_desired_throttle() > blimp.get_non_takeoff_throttle()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: throttle too high");
+        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+        return false;
+    }
+
+    if (!ignore_checks &&
+        new_flightmode->requires_GPS() &&
+        !blimp.position_ok()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name());
+        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+        return false;
+    }
+
+    // check for valid altitude if old mode did not require it but new one does
+    // we only want to stop changing modes if it could make things worse
+    if (!ignore_checks &&
+        !blimp.ekf_alt_ok() &&
+        flightmode->has_manual_throttle() &&
+        !new_flightmode->has_manual_throttle()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name());
+        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+        return false;
+    }
+
+    if (!new_flightmode->init(ignore_checks)) {
+        gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name());
+        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+        return false;
+    }
+
+    // perform any cleanup required by previous flight mode
+    exit_mode(flightmode, new_flightmode);
+
+    // store previous flight mode (only used by tradeheli's autorotation)
+    prev_control_mode = control_mode;
+
+    // update flight mode
+    flightmode = new_flightmode;
+    control_mode = mode;
+    control_mode_reason = reason;
+    logger.Write_Mode((uint8_t)control_mode, reason);
+    gcs().send_message(MSG_HEARTBEAT);
+
+    // update notify object
+    notify_flight_mode();
+
+    // return success
+    return true;
+}
+
+bool Blimp::set_mode(const uint8_t new_mode, const ModeReason reason)
+{
+    static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
+#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
+    if (reason == ModeReason::GCS_COMMAND && blimp.failsafe.radio) {
+        // don't allow mode changes while in radio failsafe
+        return false;
+    }
+#endif
+    return blimp.set_mode(static_cast<Mode::Number>(new_mode), reason);
+}
+
+// update_flight_mode - calls the appropriate attitude controllers based on flight mode
+// called at 100hz or more
+void Blimp::update_flight_mode()
+{
+    // surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used
+
+    flightmode->run();
+}
+
+// exit_mode - high level call to organise cleanup as a flight mode is exited
+void Blimp::exit_mode(Mode *&old_flightmode,
+                      Mode *&new_flightmode)
+{
+
+    // smooth throttle transition when switching from manual to automatic flight modes
+    if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) {
+        // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
+        // set_accel_throttle_I_from_pilot_throttle();
+    }
+
+    // cancel any takeoffs in progress
+    // old_flightmode->takeoff_stop();
+}
+
+// notify_flight_mode - sets notify object based on current flight mode.  Only used for OreoLED notify device
+void Blimp::notify_flight_mode()
+{
+    AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
+    AP_Notify::flags.flight_mode = (uint8_t)control_mode;
+    notify.set_flight_mode_str(flightmode->name4());
+}
+
+void Mode::update_navigation()
+{
+    // run autopilot to make high level decisions about control modes
+    run_autopilot();
+}
+
+// returns desired angle in centi-degrees
+void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const
+{
+    // throttle failsafe check
+    if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) {
+        right_out = 0;
+        front_out = 0;
+        return;
+    }
+    // fetch roll and pitch inputs
+    right_out = channel_right->get_control_in();
+    front_out = channel_front->get_control_in();
+
+
+    // // do circular limit
+    // float total_in = norm(pitch_out, roll_out);
+    // if (total_in > angle_limit) {
+    //     float ratio = angle_limit / total_in;
+    //     roll_out *= ratio;
+    //     pitch_out *= ratio;
+    // }
+
+    // do lateral tilt to euler roll conversion
+    // roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));
+
+    // roll_out and pitch_out are returned
+}
+
+// bool Mode::_TakeOff::triggered(const float target_climb_rate) const
+// {
+//     if (!blimp.ap.land_complete) {
+//         // can't take off if we're already flying
+//         return false;
+//     }
+//     if (target_climb_rate <= 0.0f) {
+//         // can't takeoff unless we want to go up...
+//         return false;
+//     }
+
+//     if (blimp.motors->get_spool_state() != Fins::SpoolState::THROTTLE_UNLIMITED) {
+//         // hold aircraft on the ground until rotor speed runup has finished
+//         return false;
+//     }
+
+//     return true;
+// }
+
+bool Mode::is_disarmed_or_landed() const
+{
+    if (!motors->armed() || !blimp.ap.auto_armed || blimp.ap.land_complete) {
+        return true;
+    }
+    return false;
+}
+
+void Mode::zero_throttle_and_relax_ac(bool spool_up)
+{
+    if (spool_up) {
+        motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED);
+    } else {
+        motors->set_desired_spool_state(Fins::DesiredSpoolState::SHUT_DOWN);
+    }
+}
+
+// void Mode::zero_throttle_and_hold_attitude()
+// {
+//     // run attitude controller
+//     attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
+//     attitude_control->set_throttle_out(0.0f, false, blimp.g.throttle_filt);
+// }
+
+// void Mode::make_safe_spool_down()
+// {
+//     // command aircraft to initiate the shutdown process
+//     motors->set_desired_spool_state(Fins::DesiredSpoolState::GROUND_IDLE);
+//     switch (motors->get_spool_state()) {
+
+//     case Fins::SpoolState::SHUT_DOWN:
+//     case Fins::SpoolState::GROUND_IDLE:
+//         // relax controllers during idle states
+//         // attitude_control->reset_rate_controller_I_terms_smoothly();
+//         // attitude_control->set_yaw_target_to_current_heading();
+//         break;
+
+//     case Fins::SpoolState::SPOOLING_UP:
+//     case Fins::SpoolState::THROTTLE_UNLIMITED:
+//     case Fins::SpoolState::SPOOLING_DOWN:
+//         // while transitioning though active states continue to operate normally
+//         break;
+//     }
+
+//     // pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
+//     // pos_control->update_z_controller();
+//     // we may need to move this out
+//     // attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
+// }
+
+/*
+  get a height above ground estimate for landing
+ */
+int32_t Mode::get_alt_above_ground_cm(void)
+{
+    int32_t alt_above_ground_cm;
+    // if (blimp.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) {
+    //     return alt_above_ground_cm;
+    // }
+    // if (!pos_control->is_active_xy()) {
+    //     return blimp.current_loc.alt;
+    // }
+    if (blimp.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) {
+        return alt_above_ground_cm;
+    }
+
+    // Assume the Earth is flat:
+    return blimp.current_loc.alt;
+}
+
+// void Mode::land_run_vertical_control(bool pause_descent)
+// {
+//     float cmb_rate = 0;
+//     if (!pause_descent) {
+//         float max_land_descent_velocity;
+//         if (g.land_speed_high > 0) {
+//             max_land_descent_velocity = -g.land_speed_high;
+//         } else {
+//             max_land_descent_velocity = pos_control->get_max_speed_down();
+//         }
+
+//         // Don't speed up for landing.
+//         max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
+
+//         // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
+//         // cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt);
+
+//         // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
+//         // cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
+//     }
+
+//     // update altitude target and call position controller
+//     pos_control->set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true);
+//     pos_control->update_z_controller();
+// }
+
+// void Mode::land_run_horizontal_control()
+// {
+//     float target_roll = 0.0f;
+//     float target_pitch = 0.0f;
+//     float target_yaw_rate = 0;
+
+//     // relax loiter target if we might be landed
+//     if (blimp.ap.land_complete_maybe) {
+//         loiter_nav->soften_for_landing();
+//     }
+
+//     // process pilot inputs
+//     if (!blimp.failsafe.radio) {
+//         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && blimp.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+//             AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
+//             // exit land if throttle is high
+//             if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) {
+//                 set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
+//             }
+//         }
+
+//         if (g.land_repositioning) {
+
+//             // convert pilot input to lean angles
+//             get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+
+//             // record if pilot has overridden roll or pitch
+//             if (!is_zero(target_roll) || !is_zero(target_pitch)) {
+//                 if (!blimp.ap.land_repo_active) {
+//                     AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
+//                 }
+//                 blimp.ap.land_repo_active = true;
+//             }
+//         }
+
+//         // get pilot's desired yaw rate
+//         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+//         if (!is_zero(target_yaw_rate)) {
+//             auto_yaw.set_mode(AUTO_YAW_HOLD);
+//         }
+//     }
+
+//     // process roll, pitch inputs
+//     loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
+
+//     // run loiter controller
+//     loiter_nav->update();
+
+//     float nav_roll  = loiter_nav->get_roll();
+//     float nav_pitch = loiter_nav->get_pitch();
+
+//     if (g2.wp_navalt_min > 0) {
+//         // user has requested an altitude below which navigation
+//         // attitude is limited. This is used to prevent commanded roll
+//         // over on landing, which particularly affects heliblimps if
+//         // there is any position estimate drift after touchdown. We
+//         // limit attitude to 7 degrees below this limit and linearly
+//         // interpolate for 1m above that
+//         float attitude_limit_cd = linear_interpolate(700, blimp.aparm.angle_max, get_alt_above_ground_cm(),
+//                                                      g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);
+//         float total_angle_cd = norm(nav_roll, nav_pitch);
+//         if (total_angle_cd > attitude_limit_cd) {
+//             float ratio = attitude_limit_cd / total_angle_cd;
+//             nav_roll *= ratio;
+//             nav_pitch *= ratio;
+
+//             // tell position controller we are applying an external limit
+//             pos_control->set_limit_accel_xy();
+//         }
+//     }
+
+//     // call attitude controller
+//     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
+//         // roll & pitch from waypoint controller, yaw rate from pilot
+//         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
+//     } else {
+//         // roll, pitch from waypoint controller, yaw heading from auto_heading()
+//         attitude_control->input_euler_angle_roll_pitch_yaw(nav_roll, nav_pitch, auto_yaw.yaw(), true);
+//     }
+// }
+
+float Mode::throttle_hover() const
+{
+    return motors->get_throttle_hover();
+}
+
+// transform pilot's manual throttle input to make hover throttle mid stick
+// used only for manual throttle modes
+// thr_mid should be in the range 0 to 1
+// returns throttle output 0 to 1
+float Mode::get_pilot_desired_throttle() const
+{
+    const float thr_mid = throttle_hover();
+    int16_t throttle_control = channel_down->get_control_in();
+
+    int16_t mid_stick = blimp.get_throttle_mid();
+    // protect against unlikely divide by zero
+    if (mid_stick <= 0) {
+        mid_stick = 500;
+    }
+
+    // ensure reasonable throttle values
+    throttle_control = constrain_int16(throttle_control,0,1000);
+
+    // calculate normalised throttle input
+    float throttle_in;
+    if (throttle_control < mid_stick) {
+        throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
+    } else {
+        throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
+    }
+
+    const float expo = constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f);
+    // calculate the output throttle using the given expo function
+    float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
+    return throttle_out;
+}
+
+// Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
+// {
+//     // Alt Hold State Machine Determination
+//     if (!motors->armed()) {
+//         // the aircraft should moved to a shut down state
+//         motors->set_desired_spool_state(Fins::DesiredSpoolState::SHUT_DOWN);
+
+//         // transition through states as aircraft spools down
+//         switch (motors->get_spool_state()) {
+
+//         case Fins::SpoolState::SHUT_DOWN:
+//             return AltHold_MotorStopped;
+
+//         case Fins::SpoolState::GROUND_IDLE:
+//             return AltHold_Landed_Ground_Idle;
+
+//         default:
+//             return AltHold_Landed_Pre_Takeoff;
+//         }
+
+//     } else if (takeoff.running() || takeoff.triggered(target_climb_rate_cms)) {
+//         // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
+//         // the aircraft should progress through the take off procedure
+//         return AltHold_Takeoff;
+
+//     } else if (!blimp.ap.auto_armed || blimp.ap.land_complete) {
+//         // the aircraft is armed and landed
+//         if (target_climb_rate_cms < 0.0f && !blimp.ap.using_interlock) {
+//             // the aircraft should move to a ground idle state
+//             motors->set_desired_spool_state(Fins::DesiredSpoolState::GROUND_IDLE);
+
+//         } else {
+//             // the aircraft should prepare for imminent take off
+//             motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED);
+//         }
+
+//         if (motors->get_spool_state() == Fins::SpoolState::GROUND_IDLE) {
+//             // the aircraft is waiting in ground idle
+//             return AltHold_Landed_Ground_Idle;
+
+//         } else {
+//             // the aircraft can leave the ground at any time
+//             return AltHold_Landed_Pre_Takeoff;
+//         }
+
+//     } else {
+//         // the aircraft is in a flying state
+//         motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED);
+//         return AltHold_Flying;
+//     }
+// }
+
+// pass-through functions to reduce code churn on conversion;
+// these are candidates for moving into the Mode base
+// class.
+float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
+{
+    return blimp.get_pilot_desired_yaw_rate(stick_angle);
+}
+
+float Mode::get_pilot_desired_climb_rate(float throttle_control)
+{
+    return blimp.get_pilot_desired_climb_rate(throttle_control);
+}
+
+float Mode::get_non_takeoff_throttle()
+{
+    return blimp.get_non_takeoff_throttle();
+}
+
+bool Mode::set_mode(Mode::Number mode, ModeReason reason)
+{
+    return blimp.set_mode(mode, reason);
+}
+
+void Mode::set_land_complete(bool b)
+{
+    return blimp.set_land_complete(b);
+}
+
+GCS_Blimp &Mode::gcs()
+{
+    return blimp.gcs();
+}
+
+// set_throttle_takeoff - allows modes to tell throttle controller we
+// are taking off so I terms can be cleared
+// void Mode::set_throttle_takeoff()
+// {
+//     // tell position controller to reset alt target and reset I terms
+//     pos_control->init_takeoff();
+// }
+
+uint16_t Mode::get_pilot_speed_dn()
+{
+    return blimp.get_pilot_speed_dn();
+}

Blimp/mode.h

@@ -0,0 +1,374 @@
+#pragma once
+
+#include "Blimp.h"
+class Parameters;
+class ParametersG2;
+
+class GCS_Blimp;
+
+class Mode
+{
+
+public:
+
+    // Auto Pilot Modes enumeration
+    enum class Number : uint8_t {
+        MANUAL =        0,  // manual control similar to Copter's stabilize mode
+        LAND =          1,  // currently just stops moving
+        // STABILIZE =     0,  // manual airframe angle with manual throttle
+        // ACRO =          1,  // manual body-frame angular rate with manual throttle
+        // ALT_HOLD =      2,  // manual airframe angle with automatic throttle
+        // AUTO =          3,  // fully automatic waypoint control using mission commands
+        // GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
+        // LOITER =        5,  // automatic horizontal acceleration with automatic throttle
+        // RTL =           6,  // automatic return to launching point
+        // CIRCLE =        7,  // automatic circular flight with automatic throttle
+        // LAND =          9,  // automatic landing with horizontal position control
+        // DRIFT =        11,  // semi-automous position, yaw and throttle control
+        // SPORT =        13,  // manual earth-frame angular rate control with manual throttle
+        // FLIP =         14,  // automatically flip the vehicle on the roll axis
+        // AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
+        // POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
+        // BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
+        // THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
+        // AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
+        // GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
+        // SMART_RTL =    21,  // SMART_RTL returns to home by retracing its steps
+        // FLOWHOLD  =    22,  // FLOWHOLD holds position with optical flow without rangefinder
+        // FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
+        // ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
+        // SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
+        // AUTOROTATE =   26,  // Autonomous autorotation
+    };
+
+    // constructor
+    Mode(void);
+
+    // do not allow copying
+    Mode(const Mode &other) = delete;
+    Mode &operator=(const Mode&) = delete;
+
+    // child classes should override these methods
+    virtual bool init(bool ignore_checks)
+    {
+        return true;
+    }
+    virtual void run() = 0;
+    virtual bool requires_GPS() const = 0;
+    virtual bool has_manual_throttle() const = 0;
+    virtual bool allows_arming(bool from_gcs) const = 0;
+    virtual bool is_autopilot() const
+    {
+        return false;
+    }
+    virtual bool has_user_takeoff(bool must_navigate) const
+    {
+        return false;
+    }
+    virtual bool in_guided_mode() const
+    {
+        return false;
+    }
+    virtual bool logs_attitude() const
+    {
+        return false;
+    }
+
+    // return a string for this flightmode
+    virtual const char *name() const = 0;
+    virtual const char *name4() const = 0;
+
+    bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate);
+    // virtual bool is_taking_off() const;
+    // static void takeoff_stop() { takeoff.stop(); }
+
+    virtual bool is_landing() const
+    {
+        return false;
+    }
+
+    // mode requires terrain to be present to be functional
+    virtual bool requires_terrain_failsafe() const
+    {
+        return false;
+    }
+
+    // functions for reporting to GCS
+    virtual bool get_wp(Location &loc)
+    {
+        return false;
+    };
+    virtual int32_t wp_bearing() const
+    {
+        return 0;
+    }
+    virtual uint32_t wp_distance() const
+    {
+        return 0;
+    }
+    virtual float crosstrack_error() const
+    {
+        return 0.0f;
+    }
+
+    void update_navigation();
+
+    int32_t get_alt_above_ground_cm(void);
+
+    // pilot input processing
+    void get_pilot_desired_accelerations(float &right_out, float &front_out) const;
+    float get_pilot_desired_yaw_rate(int16_t stick_angle);
+    float get_pilot_desired_throttle() const;
+
+    // returns climb target_rate reduced to avoid obstacles and
+    // altitude fence
+    float get_avoidance_adjusted_climbrate(float target_rate);
+
+    // const Vector3f& get_desired_velocity() {
+    //     // note that position control isn't used in every mode, so
+    //     // this may return bogus data:
+    //     return pos_control->get_desired_velocity();
+    // }
+
+protected:
+
+    // navigation support functions
+    virtual void run_autopilot() {}
+
+    // helper functions
+    bool is_disarmed_or_landed() const;
+    void zero_throttle_and_relax_ac(bool spool_up = false);
+    void zero_throttle_and_hold_attitude();
+    void make_safe_spool_down();
+
+    // functions to control landing
+    // in modes that support landing
+    void land_run_horizontal_control();
+    void land_run_vertical_control(bool pause_descent = false);
+
+    // return expected input throttle setting to hover:
+    virtual float throttle_hover() const;
+
+    // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
+    // enum AltHoldModeState {
+    //     AltHold_MotorStopped,
+    //     AltHold_Takeoff,
+    //     AltHold_Landed_Ground_Idle,
+    //     AltHold_Landed_Pre_Takeoff,
+    //     AltHold_Flying
+    // };
+    // AltHoldModeState get_alt_hold_state(float target_climb_rate_cms);
+
+    // convenience references to avoid code churn in conversion:
+    Parameters &g;
+    ParametersG2 &g2;
+    // AC_WPNav *&wp_nav;
+    // AC_Loiter *&loiter_nav;
+    AP_InertialNav &inertial_nav;
+    AP_AHRS &ahrs;
+    // AC_AttitudeControl_t *&attitude_control;
+    Fins *&motors;
+    // Fins *&motors;
+    RC_Channel *&channel_right;
+    RC_Channel *&channel_front;
+    RC_Channel *&channel_down;
+    RC_Channel *&channel_yaw;
+    float &G_Dt;
+
+    // note that we support two entirely different automatic takeoffs:
+
+    // "user-takeoff", which is available in modes such as ALT_HOLD
+    // (see has_user_takeoff method).  "user-takeoff" is a simple
+    // reach-altitude-based-on-pilot-input-or-parameter routine.
+
+    // "auto-takeoff" is used by both Guided and Auto, and is
+    // basically waypoint navigation with pilot yaw permitted.
+
+    // user-takeoff support; takeoff state is shared across all mode instances
+    // class _TakeOff {
+    // public:
+    //     void start(float alt_cm);
+    //     void stop();
+    //     void get_climb_rates(float& pilot_climb_rate,
+    //                          float& takeoff_climb_rate);
+    //     bool triggered(float target_climb_rate) const;
+
+    //     bool running() const { return _running; }
+    // private:
+    //     bool _running;
+    //     float max_speed;
+    //     float alt_delta;
+    //     uint32_t start_ms;
+    // };
+
+    // static _TakeOff takeoff;
+
+    // virtual bool do_user_takeoff_start(float takeoff_alt_cm);
+
+    // method shared by both Guided and Auto for takeoff.  This is
+    // waypoint navigation but the user can control the yaw.
+    // void auto_takeoff_run();
+    // void auto_takeoff_set_start_alt(void);
+
+    // altitude above-ekf-origin below which auto takeoff does not control horizontal position
+    // static bool auto_takeoff_no_nav_active;
+    // static float auto_takeoff_no_nav_alt_cm;
+
+public:
+    // Navigation Yaw control
+    class AutoYaw
+    {
+
+    public:
+
+        // yaw(): main product of AutoYaw; the heading:
+        float yaw();
+
+        // mode(): current method of determining desired yaw:
+        autopilot_yaw_mode mode() const
+        {
+            return (autopilot_yaw_mode)_mode;
+        }
+        void set_mode_to_default(bool rtl);
+        void set_mode(autopilot_yaw_mode new_mode);
+        autopilot_yaw_mode default_mode(bool rtl) const;
+
+        // rate_cds(): desired yaw rate in centidegrees/second:
+        float rate_cds() const;
+        void set_rate(float new_rate_cds);
+
+        // set_roi(...): set a "look at" location:
+        void set_roi(const Location &roi_location);
+
+        void set_fixed_yaw(float angle_deg,
+                           float turn_rate_dps,
+                           int8_t direction,
+                           bool relative_angle);
+
+    private:
+
+        float look_ahead_yaw();
+        float roi_yaw();
+
+        // auto flight mode's yaw mode
+        uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
+
+        // Yaw will point at this location if mode is set to AUTO_YAW_ROI
+        Vector3f roi;
+
+        // bearing from current location to the ROI
+        float _roi_yaw;
+
+        // yaw used for YAW_FIXED yaw_mode
+        int32_t _fixed_yaw;
+
+        // Deg/s we should turn
+        int16_t _fixed_yaw_slewrate;
+
+        // heading when in yaw_look_ahead_yaw
+        float _look_ahead_yaw;
+
+        // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
+        float _rate_cds;
+
+        // used to reduce update rate to 100hz:
+        uint8_t roi_yaw_counter;
+
+    };
+    static AutoYaw auto_yaw;
+
+    // pass-through functions to reduce code churn on conversion;
+    // these are candidates for moving into the Mode base
+    // class.
+    float get_pilot_desired_climb_rate(float throttle_control);
+    float get_non_takeoff_throttle(void);
+    bool set_mode(Mode::Number mode, ModeReason reason);
+    void set_land_complete(bool b);
+    GCS_Blimp &gcs();
+    void set_throttle_takeoff(void);
+    uint16_t get_pilot_speed_dn(void);
+
+    // end pass-through functions
+};
+
+class ModeManual : public Mode
+{
+
+public:
+    // inherit constructor
+    using Mode::Mode;
+
+    virtual void run() override;
+
+    bool requires_GPS() const override
+    {
+        return false;
+    }
+    bool has_manual_throttle() const override
+    {
+        return true;
+    }
+    bool allows_arming(bool from_gcs) const override
+    {
+        return true;
+    };
+    bool is_autopilot() const override
+    {
+        return false;
+    }
+
+protected:
+
+    const char *name() const override
+    {
+        return "MANUAL";
+    }
+    const char *name4() const override
+    {
+        return "MANU";
+    }
+
+private:
+
+};
+
+class ModeLand : public Mode
+{
+
+public:
+    // inherit constructor
+    using Mode::Mode;
+
+    virtual void run() override;
+
+    bool requires_GPS() const override
+    {
+        return false;
+    }
+    bool has_manual_throttle() const override
+    {
+        return true;
+    }
+    bool allows_arming(bool from_gcs) const override
+    {
+        return false;
+    };
+    bool is_autopilot() const override
+    {
+        return false;
+    }
+
+protected:
+
+    const char *name() const override
+    {
+        return "LAND";
+    }
+    const char *name4() const override
+    {
+        return "LAND";
+    }
+
+private:
+
+};

Blimp/mode_land.cpp

@@ -0,0 +1,24 @@
+#include "Blimp.h"
+
+/*
+ * Init and run calls for stabilize flight mode
+ */
+
+// manual_run - runs the main manual controller
+// should be called at 100hz or more
+void ModeLand::run()
+{
+    //stop moving
+
+}
+
+// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
+//  this is always called from a failsafe so we trigger notification to pilot
+void Blimp::set_mode_land_with_pause(ModeReason reason)
+{
+    set_mode(Mode::Number::LAND, reason);
+    // land_pause = true;
+
+    // alert pilot to mode change
+    AP_Notify::events.failsafe_mode_change = 1;
+}

Blimp/mode_manual.cpp

@@ -0,0 +1,29 @@
+#include "Blimp.h"
+/*
+ * Init and run calls for stabilize flight mode
+ */
+
+// manual_run - runs the main manual controller
+// should be called at 100hz or more
+void ModeManual::run()
+{
+    // convert pilot input to lean angles
+    // float target_right, target_front;
+    // get_pilot_desired_accelerations(target_right, target_front);
+
+    motors->right_out = channel_right->get_control_in();
+    motors->front_out = channel_front->get_control_in();
+    motors->yaw_out = channel_yaw->get_control_in();
+    motors->down_out = channel_down->get_control_in();
+
+    if (!motors->armed()) {
+        // Motors should be Stopped
+        motors->set_desired_spool_state(Fins::DesiredSpoolState::SHUT_DOWN);
+    } else {
+        motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED);
+    }
+
+
+
+    // motors->output(); //MIR need to add sending direction & throttle commands.
+}

Blimp/motors.cpp

@@ -0,0 +1,92 @@
+#include "Blimp.h"
+
+#define ARM_DELAY               20  // called at 10hz so 2 seconds
+#define DISARM_DELAY            20  // called at 10hz so 2 seconds
+#define AUTO_TRIM_DELAY         100 // called at 10hz so 10 seconds
+#define LOST_VEHICLE_DELAY      10  // called at 10hz so 1 second
+
+// arm_motors_check - checks for pilot input to arm or disarm the blimp
+// called at 10hz
+void Blimp::arm_motors_check()
+{
+    static int16_t arming_counter;
+
+    // check if arming/disarm using rudder is allowed
+    AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
+    if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
+        return;
+    }
+
+    // ensure throttle is down
+    if (channel_down->get_control_in() > 0) { //MIR what dow we do with this?
+        arming_counter = 0;
+        return;
+    }
+
+    int16_t yaw_in = channel_yaw->get_control_in();
+
+    // full right
+    if (yaw_in > 900) {
+
+        // increase the arming counter to a maximum of 1 beyond the auto trim counter
+        if (arming_counter <= AUTO_TRIM_DELAY) {
+            arming_counter++;
+        }
+
+        // arm the motors and configure for flight
+        if (arming_counter == ARM_DELAY && !motors->armed()) {
+            // reset arming counter if arming fail
+            if (!arming.arm(AP_Arming::Method::RUDDER)) {
+                arming_counter = 0;
+            }
+        }
+
+        // arm the motors and configure for flight
+        if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::MANUAL) {
+            gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");
+            auto_trim_counter = 250;
+            auto_trim_started = false;
+        }
+
+        // full left and rudder disarming is enabled
+    } else if ((yaw_in < -900) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
+        if (!flightmode->has_manual_throttle() && !ap.land_complete) {
+            arming_counter = 0;
+            return;
+        }
+
+        // increase the counter to a maximum of 1 beyond the disarm delay
+        if (arming_counter <= DISARM_DELAY) {
+            arming_counter++;
+        }
+
+        // disarm the motors
+        if (arming_counter == DISARM_DELAY && motors->armed()) {
+            arming.disarm(AP_Arming::Method::RUDDER);
+        }
+
+        // Yaw is centered so reset arming counter
+    } else {
+        arming_counter = 0;
+    }
+}
+
+
+// motors_output - send output to motors library which will adjust and send to ESCs and servos
+void Blimp::motors_output()
+{
+    // output any servo channels
+    SRV_Channels::calc_pwm();
+
+    // cork now, so that all channel outputs happen at once
+    SRV_Channels::cork();
+
+    // update output on any aux channels, for manual passthru
+    SRV_Channels::output_ch_all();
+
+    // send output signals to motors
+    motors->output();
+
+    // push all channels
+    SRV_Channels::push();
+}

Blimp/radio.cpp

@@ -0,0 +1,192 @@
+#include "Blimp.h"
+
+
+// Function that will read the radio data, limit servos and trigger a failsafe
+// ----------------------------------------------------------------------------
+
+void Blimp::default_dead_zones()
+{
+    channel_right->set_default_dead_zone(20);
+    channel_front->set_default_dead_zone(20);
+    channel_down->set_default_dead_zone(30);
+    channel_yaw->set_default_dead_zone(20);
+    rc().channel(CH_6)->set_default_dead_zone(0);
+}
+
+void Blimp::init_rc_in()
+{
+    channel_right     = rc().channel(rcmap.roll()-1);
+    channel_front    = rc().channel(rcmap.pitch()-1);
+    channel_down = rc().channel(rcmap.throttle()-1);
+    channel_yaw      = rc().channel(rcmap.yaw()-1);
+
+    // set rc channel ranges
+    channel_right->set_angle(RC_SCALE);
+    channel_front->set_angle(RC_SCALE);
+    channel_yaw->set_angle(RC_SCALE);
+    channel_down->set_angle(RC_SCALE);
+
+    // set default dead zones
+    default_dead_zones();
+
+    // initialise throttle_zero flag
+    ap.throttle_zero = true;
+}
+
+// init_rc_out -- initialise motors
+void Blimp::init_rc_out()
+{
+    // motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
+    // MIR will need motors->init later when we switch to other fin config.
+
+    // enable aux servos to cope with multiple output channels per motor
+    SRV_Channels::enable_aux_servos();
+
+    // update rate must be set after motors->init() to allow for motor mapping
+    // motors->set_update_rate(g.rc_speed);
+
+    // motors->set_throttle_range(channel_down->get_radio_min(), channel_down->get_radio_max());
+
+    // refresh auxiliary channel to function map
+    SRV_Channels::update_aux_servo_function();
+
+    /*
+      setup a default safety ignore mask, so that servo gimbals can be active while safety is on
+     */
+    // uint16_t safety_ignore_mask = (~blimp.motors->get_motor_mask()) & 0x3FFF;
+    // BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask);
+}
+
+
+// enable_motor_output() - enable and output lowest possible value to motors
+void Blimp::enable_motor_output()
+{
+    // enable motors
+    motors->output_min();
+}
+
+void Blimp::read_radio()
+{
+    const uint32_t tnow_ms = millis();
+
+    if (rc().read_input()) {
+        ap.new_radio_frame = true;
+
+        set_throttle_and_failsafe(channel_down->get_radio_in());
+        set_throttle_zero_flag(channel_down->get_control_in());
+
+        // RC receiver must be attached if we've just got input
+        ap.rc_receiver_present = true;
+
+        // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax blimps)
+        // radio_passthrough_to_motors();
+
+        const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
+        rc_throttle_control_in_filter.apply(channel_down->get_control_in(), dt);
+        last_radio_update_ms = tnow_ms;
+        return;
+    }
+
+    // No radio input this time
+    if (failsafe.radio) {
+        // already in failsafe!
+        return;
+    }
+
+    const uint32_t elapsed = tnow_ms - last_radio_update_ms;
+    // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
+    const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS;
+    if (elapsed < timeout) {
+        // not timed out yet
+        return;
+    }
+    if (!g.failsafe_throttle) {
+        // throttle failsafe not enabled
+        return;
+    }
+    if (!ap.rc_receiver_present && !motors->armed()) {
+        // we only failsafe if we are armed OR we have ever seen an RC receiver
+        return;
+    }
+
+    // Nobody ever talks to us.  Log an error and enter failsafe.
+    AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
+    set_failsafe_radio(true);
+}
+
+#define FS_COUNTER 3        // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
+void Blimp::set_throttle_and_failsafe(uint16_t throttle_pwm)
+{
+    // if failsafe not enabled pass through throttle and exit
+    if (g.failsafe_throttle == FS_THR_DISABLED) {
+        return;
+    }
+
+    //check for low throttle value
+    if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) {
+
+        // if we are already in failsafe or motors not armed pass through throttle and exit
+        if (failsafe.radio || !(ap.rc_receiver_present || motors->armed())) {
+            return;
+        }
+
+        // check for 3 low throttle values
+        // Note: we do not pass through the low throttle until 3 low throttle values are received
+        failsafe.radio_counter++;
+        if ( failsafe.radio_counter >= FS_COUNTER ) {
+            failsafe.radio_counter = FS_COUNTER;  // check to ensure we don't overflow the counter
+            set_failsafe_radio(true);
+        }
+    } else {
+        // we have a good throttle so reduce failsafe counter
+        failsafe.radio_counter--;
+        if ( failsafe.radio_counter <= 0 ) {
+            failsafe.radio_counter = 0;   // check to ensure we don't underflow the counter
+
+            // disengage failsafe after three (nearly) consecutive valid throttle values
+            if (failsafe.radio) {
+                set_failsafe_radio(false);
+            }
+        }
+        // pass through throttle
+    }
+}
+
+#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400
+// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control
+// throttle_zero is used to determine if the pilot intends to shut down the motors
+// Basically, this signals when we are not flying.  We are either on the ground
+// or the pilot has shut down the blimp in the air and it is free-falling
+void Blimp::set_throttle_zero_flag(int16_t throttle_control)
+{
+    static uint32_t last_nonzero_throttle_ms = 0;
+    uint32_t tnow_ms = millis();
+
+    // if not using throttle interlock and non-zero throttle and not E-stopped,
+    // or using motor interlock and it's enabled, then motors are running,
+    // and we are flying. Immediately set as non-zero
+    if (throttle_control > 0) {
+        last_nonzero_throttle_ms = tnow_ms;
+        ap.throttle_zero = false;
+    } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
+        ap.throttle_zero = true;
+    }
+    //MIR What does this mean??
+}
+
+// pass pilot's inputs to motors library (used to allow wiggling servos while disarmed on heli, single, coax blimps)
+// void Blimp::radio_passthrough_to_motors()
+// {
+//     motors->set_radio_passthrough(channel_right->norm_input(),
+//                                   channel_front->norm_input(),
+//                                   channel_down->get_control_in_zero_dz()*0.001f,
+//                                   channel_yaw->norm_input());
+// }
+
+/*
+  return the throttle input for mid-stick as a control-in value
+ */
+int16_t Blimp::get_throttle_mid(void)
+{
+    return channel_down->get_control_mid();
+}

Blimp/sensors.cpp

@@ -0,0 +1,62 @@
+#include "Blimp.h"
+
+// return barometric altitude in centimeters
+void Blimp::read_barometer(void)
+{
+    barometer.update();
+
+    baro_alt = barometer.get_altitude() * 100.0f;
+
+    motors->set_air_density_ratio(barometer.get_air_density_ratio());
+}
+
+
+void Blimp::compass_cal_update()
+{
+    compass.cal_update();
+
+    if (hal.util->get_soft_armed()) {
+        return;
+    }
+
+    static uint32_t compass_cal_stick_gesture_begin = 0;
+
+    if (compass.is_calibrating()) {
+        if (channel_yaw->get_control_in() < -4000 && channel_down->get_control_in() > 900) {
+            compass.cancel_calibration_all();
+        }
+    } else {
+        bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors->armed() && channel_yaw->get_control_in() > 4000 && channel_down->get_control_in() > 900;
+        uint32_t tnow = millis();
+
+        if (!stick_gesture_detected) {
+            compass_cal_stick_gesture_begin = tnow;
+        } else if (tnow-compass_cal_stick_gesture_begin > 1000*COMPASS_CAL_STICK_GESTURE_TIME) {
+#ifdef CAL_ALWAYS_REBOOT
+            compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,true);
+#else
+            compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,false);
+#endif
+        }
+    }
+}
+
+void Blimp::accel_cal_update()
+{
+    if (hal.util->get_soft_armed()) {
+        return;
+    }
+    ins.acal_update();
+    // check if new trim values, and set them
+    float trim_roll, trim_pitch;
+    if (ins.get_new_trim(trim_roll, trim_pitch)) {
+        ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
+    }
+
+#ifdef CAL_ALWAYS_REBOOT
+    if (ins.accel_cal_requires_reboot()) {
+        hal.scheduler->delay(1000);
+        hal.scheduler->reboot(false);
+    }
+#endif
+}

Blimp/system.cpp

@@ -0,0 +1,358 @@
+#include "Blimp.h"
+
+/*****************************************************************************
+*   The init_ardupilot function processes everything we need for an in - air restart
+*        We will determine later if we are actually on the ground and process a
+*        ground start in that case.
+*
+*****************************************************************************/
+
+static void failsafe_check_static()
+{
+    blimp.failsafe_check();
+}
+
+void Blimp::init_ardupilot()
+{
+
+#if STATS_ENABLED == ENABLED
+    // initialise stats module
+    g2.stats.init();
+#endif
+
+    BoardConfig.init();
+
+
+    // initialise notify system
+    notify.init();
+    notify_flight_mode();
+
+    // initialise battery monitor
+    battery.init();
+
+    // Init RSSI
+    rssi.init();
+
+    barometer.init();
+
+    // setup telem slots with serial ports
+    gcs().setup_uarts();
+
+#if LOGGING_ENABLED == ENABLED
+    log_init();
+#endif
+
+    // update motor interlock state
+    // update_using_interlock();
+
+
+    init_rc_in();               // sets up rc channels from radio
+
+    // allocate the motors class
+    allocate_motors();
+
+    // initialise rc channels including setting mode
+    rc().init();
+
+    // sets up motors and output to escs
+    init_rc_out();
+
+
+    // motors initialised so parameters can be sent
+    ap.initialised_params = true;
+
+    relay.init();
+
+    /*
+     *  setup the 'main loop is dead' check. Note that this relies on
+     *  the RC library being initialised.
+     */
+    hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
+
+    // Do GPS init
+    gps.set_log_gps_bit(MASK_LOG_GPS);
+    gps.init(serial_manager);
+
+    AP::compass().set_log_bit(MASK_LOG_COMPASS);
+    AP::compass().init();
+
+    // attitude_control->parameter_sanity_check();
+    // pos_control->set_dt(scheduler.get_loop_period_s());
+
+#if HIL_MODE != HIL_MODE_DISABLED
+    while (barometer.get_last_update() == 0) {
+        // the barometer begins updating when we get the first
+        // HIL_STATE message
+        gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
+        delay(1000);
+    }
+
+    // set INS to HIL mode
+    ins.set_hil_mode();
+#endif
+
+    // read Baro pressure at ground
+    //-----------------------------
+    barometer.set_log_baro_bit(MASK_LOG_IMU);
+    barometer.calibrate();
+
+    // #if MODE_AUTO_ENABLED == ENABLED
+    //     // initialise mission library
+    //     mode_auto.mission.init();
+    // #endif
+
+    // initialise AP_Logger library
+    logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&blimp, &Blimp::Log_Write_Vehicle_Startup_Messages, void));
+
+    startup_INS_ground();
+
+#ifdef ENABLE_SCRIPTING
+    g2.scripting.init();
+#endif // ENABLE_SCRIPTING
+
+    // set landed flags
+    // set_land_complete(true);
+    // set_land_complete_maybe(true);
+
+    // we don't want writes to the serial port to cause us to pause
+    // mid-flight, so set the serial ports non-blocking once we are
+    // ready to fly
+    serial_manager.set_blocking_writes_all(false);
+
+    // enable CPU failsafe
+    // failsafe_enable();
+
+    ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
+
+    // setup fin output
+    motors->setup_fins();
+
+    // enable output to motors
+    if (arming.rc_calibration_checks(true)) {
+        enable_motor_output();
+    }
+
+    // attempt to switch to MANUAL, if this fails then switch to Land
+    if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) {
+        // set mode to MANUAL will trigger mode change notification to pilot
+        set_mode(Mode::Number::MANUAL, ModeReason::UNAVAILABLE);
+    } else {
+        // alert pilot to mode change
+        AP_Notify::events.failsafe_mode_change = 1;
+    }
+
+    // flag that initialisation has completed
+    ap.initialised = true;
+}
+
+
+//******************************************************************************
+//This function does all the calibrations, etc. that we need during a ground start
+//******************************************************************************
+void Blimp::startup_INS_ground()
+{
+    // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
+    ahrs.init();
+    ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
+
+    // Warm up and calibrate gyro offsets
+    ins.init(scheduler.get_loop_rate_hz());
+
+    // reset ahrs including gyro bias
+    ahrs.reset();
+}
+
+// position_ok - returns true if the horizontal absolute position is ok and home position is set
+bool Blimp::position_ok() const
+{
+    // return false if ekf failsafe has triggered
+    if (failsafe.ekf) {
+        return false;
+    }
+
+    // check ekf position estimate
+    return (ekf_has_absolute_position() || ekf_has_relative_position());
+}
+
+// ekf_has_absolute_position - returns true if the EKF can provide an absolute WGS-84 position estimate
+bool Blimp::ekf_has_absolute_position() const
+{
+    if (!ahrs.have_inertial_nav()) {
+        // do not allow navigation with dcm position
+        return false;
+    }
+
+    // with EKF use filter status and ekf check
+    nav_filter_status filt_status = inertial_nav.get_filter_status();
+
+    // if disarmed we accept a predicted horizontal position
+    if (!motors->armed()) {
+        return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
+    } else {
+        // once armed we require a good absolute position and EKF must not be in const_pos_mode
+        return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
+    }
+}
+
+// ekf_has_relative_position - returns true if the EKF can provide a position estimate relative to it's starting position
+bool Blimp::ekf_has_relative_position() const
+{
+    // return immediately if EKF not used
+    if (!ahrs.have_inertial_nav()) {
+        return false;
+    }
+
+    // return immediately if neither optflow nor visual odometry is enabled
+    bool enabled = false;
+    // #if OPTFLOW == ENABLED
+    //     if (optflow.enabled()) {
+    //         enabled = true;
+    //     }
+    // #endif
+    // #if HAL_VISUALODOM_ENABLED
+    //     if (visual_odom.enabled()) {
+    //         enabled = true;
+    //     }
+    // #endif
+    if (!enabled) {
+        return false;
+    }
+
+    // get filter status from EKF
+    nav_filter_status filt_status = inertial_nav.get_filter_status();
+
+    // if disarmed we accept a predicted horizontal relative position
+    if (!motors->armed()) {
+        return (filt_status.flags.pred_horiz_pos_rel);
+    } else {
+        return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
+    }
+}
+
+// returns true if the ekf has a good altitude estimate (required for modes which do AltHold)
+bool Blimp::ekf_alt_ok() const
+{
+    if (!ahrs.have_inertial_nav()) {
+        // do not allow alt control with only dcm
+        return false;
+    }
+
+    // with EKF use filter status and ekf check
+    nav_filter_status filt_status = inertial_nav.get_filter_status();
+
+    // require both vertical velocity and position
+    return (filt_status.flags.vert_vel && filt_status.flags.vert_pos);
+}
+
+// update_auto_armed - update status of auto_armed flag
+void Blimp::update_auto_armed()
+{
+    // disarm checks
+    if (ap.auto_armed) {
+        // if motors are disarmed, auto_armed should also be false
+        if (!motors->armed()) {
+            set_auto_armed(false);
+            return;
+        }
+        // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
+        if (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) {
+            set_auto_armed(false);
+        }
+        // if heliblimps are on the ground, and the motor is switched off, auto-armed should be false
+        // so that rotor runup is checked again before attempting to take-off
+        if (ap.land_complete && motors->get_spool_state() != Fins::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) {
+            set_auto_armed(false);
+        }
+    } else {
+        // arm checks
+
+        // // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
+        // if(motors->armed() && ap.using_interlock) {
+        //     if(!ap.throttle_zero && motors->get_spool_state() == Fins::SpoolState::THROTTLE_UNLIMITED) {
+        //         set_auto_armed(true);
+        //     }
+        // // if motors are armed and throttle is above zero auto_armed should be true
+        // // if motors are armed and we are in throw mode, then auto_armed should be true
+        // } else if (motors->armed() && !ap.using_interlock) {
+        //     if(!ap.throttle_zero) {
+        //         set_auto_armed(true);
+        //     }
+        // }
+    }
+}
+
+/*
+  should we log a message type now?
+ */
+bool Blimp::should_log(uint32_t mask)
+{
+#if LOGGING_ENABLED == ENABLED
+    ap.logging_started = logger.logging_started();
+    return logger.should_log(mask);
+#else
+    return false;
+#endif
+}
+
+// return MAV_TYPE corresponding to frame class
+MAV_TYPE Blimp::get_frame_mav_type()
+{
+    return MAV_TYPE_QUADROTOR; //MIR changed to this for now - will need to deal with mavlink changes later
+}
+
+// return string corresponding to frame_class
+const char* Blimp::get_frame_string()
+{
+    return "AIRFISH";
+}
+
+/*
+  allocate the motors class
+ */
+void Blimp::allocate_motors(void)
+{
+    switch ((Fins::motor_frame_class)g2.frame_class.get()) {
+    case Fins::MOTOR_FRAME_AIRFISH:
+    default:
+        motors = new Fins(blimp.scheduler.get_loop_rate_hz());
+        break;
+    }
+    if (motors == nullptr) {
+        AP_HAL::panic("Unable to allocate FRAME_CLASS=%u", (unsigned)g2.frame_class.get());
+    }
+    AP_Param::load_object_from_eeprom(motors, Fins::var_info);
+
+    // const struct AP_Param::GroupInfo *ac_var_info;
+
+    // attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
+    // ac_var_info = AC_AttitudeControl_Multi::var_info;
+    // if (attitude_control == nullptr) {
+    //     AP_HAL::panic("Unable to allocate AttitudeControl");
+    // }
+    // AP_Param::load_object_from_eeprom(attitude_control, ac_var_info);
+
+    // pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
+    // if (pos_control == nullptr) {
+    //     AP_HAL::panic("Unable to allocate PosControl");
+    // }
+    // AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info);
+
+    // wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+
+    // if (wp_nav == nullptr) {
+    //     AP_HAL::panic("Unable to allocate WPNav");
+    // }
+    // AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info);
+
+    // loiter_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+    // if (loiter_nav == nullptr) {
+    //     AP_HAL::panic("Unable to allocate LoiterNav");
+    // }
+    // AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
+
+    // reload lines from the defaults file that may now be accessible
+    AP_Param::reload_defaults_file(true);
+
+    // param count could have changed
+    AP_Param::invalidate_count();
+}

Blimp/version.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#ifndef FORCE_VERSION_H_INCLUDE
+#error version.h should never be included directly. You probably want to include AP_Common/AP_FWVersion.h
+#endif
+
+#include "ap_version.h"
+
+#define THISFIRMWARE "Blimp V4.1.0-dev"
+
+// the following line is parsed by the autotest scripts
+#define FIRMWARE_VERSION 4,1,0,FIRMWARE_VERSION_TYPE_DEV
+
+#define FW_MAJOR 4
+#define FW_MINOR 1
+#define FW_PATCH 0
+#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+
+#include <AP_Common/AP_FWVersionDefine.h>

Blimp/wscript

@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    vehicle = bld.path.name
+    bld.ap_stlib(
+        name=vehicle + '_libs',
+        ap_vehicle=vehicle,
+        ap_libraries=bld.ap_common_vehicle_libraries() + [
+            'AC_AttitudeControl',
+            'AC_InputManager',
+            'AC_WPNav',
+            'AP_InertialNav',
+            'AP_RCMapper',
+            'AP_Avoidance',
+            'AP_Arming',
+            'AP_LTM_Telem',
+            'AP_Devo_Telem',
+            'AP_OSD',
+            'AP_KDECAN',
+            'AP_Beacon',
+            'AP_AdvancedFailsafe', #for some reason compiling GCS_Common.cpp (in libraries) fails if this isn't included...
+        ],
+    )
+
+    bld.ap_program(
+        program_name='blimp',
+        program_groups=['bin', 'blimp'],
+        use=vehicle + '_libs',
+        defines=['FRAME_CONFIG=MULTICOPTER_FRAME'],
+        )