Added sensor disabling RC options for 4.0 (NO NOT MERGE)

ArduCopter/motors.cpp

@@ -7,6 +7,10 @@
 
 static uint32_t auto_disarm_begin;
 
+#define MOTOR_COUNT  4
+static bool motor_kill_all = false;
+static bool motor_kill[MOTOR_COUNT] {};
+
 // arm_motors_check - checks for pilot input to arm or disarm the copter
 // called at 10hz
 void Copter::arm_motors_check()
@@ -176,10 +180,26 @@ void Copter::motors_output()
         motors->output();
     }
 
+    // implement motor kill for parachute testing
+    for (uint8_t i = 0; i < MOTOR_COUNT; ++i) {
+        if (motor_kill_all || motor_kill[i]) {
+            hal.rcout->write(i, 1000);
+        }
+    }
+
     // push all channels
     SRV_Channels::push();
 }
 
+void set_motor_kill(uint8_t motor_num, bool kill)
+{
+    if (motor_num == 0xFF) {
+        motor_kill_all = kill;
+    } else if (motor_num < MOTOR_COUNT) {
+        motor_kill[motor_num] = kill;
+    }
+}
+
 // check for pilot stick input to trigger lost vehicle alarm
 void Copter::lost_vehicle_check()
 {

libraries/AP_Compass/AP_Compass.cpp

@@ -1541,6 +1541,18 @@ Compass::get_healthy_mask() const
     return healthy_mask;
 }
 
+bool
+Compass::healthy(void) const
+{
+    bool health = true;
+    for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; ++i) {
+        if (use_for_yaw(i)) {
+            health &= healthy(i);
+        }
+    }
+    return health;
+}
+
 void
 Compass::set_offsets(uint8_t i, const Vector3f &offsets)
 {

libraries/AP_Compass/AP_Compass.h

@@ -183,7 +183,7 @@ friend class AP_Compass_Backend;
 
     /// Return the health of a compass
     bool healthy(uint8_t i) const { return _get_state(Priority(i)).healthy; }
-    bool healthy(void) const { return healthy(0); }
+    bool healthy(void) const;
     uint8_t get_healthy_mask() const;
 
     /// Returns the current offset values
@@ -341,6 +341,11 @@ friend class AP_Compass_Backend;
     MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
                                  float lat_deg, float lon_deg);
 
+    // set a mask to disable sensor for failure testing
+    void set_disable_mask(uint8_t mask) {
+        _disable_mask = mask;
+    }
+
 private:
     static Compass *_singleton;
 
@@ -593,6 +598,9 @@ friend class AP_Compass_Backend;
     ///
     void try_set_initial_location();
     bool _initial_location_set;
+
+    // mask of disabled sensors
+    uint8_t _disable_mask;
 };
 
 namespace AP {

libraries/AP_Compass/AP_Compass_AK09916.cpp

@@ -210,6 +210,9 @@ bool AP_Compass_AK09916::init()
         goto fail;
     }
 
+    // one checked register for mode
+    _bus->setup_checked_registers(1);
+
     if (!_setup_mode()) {
         hal.console->printf("AK09916: Could not setup mode\n");
         goto fail;
@@ -268,28 +271,31 @@ void AP_Compass_AK09916::_update()
     Vector3f raw_field;
 
     if (!_bus->block_read(REG_ST1, (uint8_t *) &regs, sizeof(regs))) {
-        return;
+        goto register_check;
     }
 
     if (!(regs.st1 & 0x01)) {
-        return;
+        goto register_check;
     }
 
     /* Check for overflow. See AK09916's datasheet*/
     if ((regs.st2 & 0x08)) {
-        return;
+        goto register_check;
     }
 
     raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
 
     if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
-        return;
+        goto register_check;
     }
 
     _make_adc_sensitivity_adjustment(raw_field);
     raw_field *= AK09916_MILLIGAUSS_SCALE;
 
     accumulate_sample(raw_field, _compass_instance, 10);
+
+register_check:
+    _bus->check_next_register();
 }
 
 bool AP_Compass_AK09916::_check_id()
@@ -314,7 +320,7 @@ bool AP_Compass_AK09916::_check_id()
 }
 
 bool AP_Compass_AK09916::_setup_mode() {
-    return _bus->register_write(REG_CNTL2, 0x08); //Continuous Mode 2
+    return _bus->register_write(REG_CNTL2, 0x08, true); //Continuous Mode 2
 }
 
 bool AP_Compass_AK09916::_reset()
@@ -338,9 +344,9 @@ bool AP_AK09916_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
     return _dev->read_registers(reg, val, 1);
 }
 
-bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
+bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val, bool checked)
 {
-    return _dev->write_register(reg, val);
+    return _dev->write_register(reg, val, checked);
 }
 
 AP_HAL::Semaphore *AP_AK09916_BusDriver_HALDevice::get_semaphore()
@@ -400,8 +406,9 @@ bool AP_AK09916_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
     return _slave->passthrough_read(reg, val, 1) == 1;
 }
 
-bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
+bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val, bool checked)
 {
+    (void)checked;
     return _slave->passthrough_write(reg, val) == 1;
 }
 

libraries/AP_Compass/AP_Compass_AK09916.h

@@ -77,7 +77,6 @@ class AP_Compass_AK09916 : public AP_Compass_Backend
 
     AP_AK09916_BusDriver *_bus;
 
-    float _magnetometer_ASA[3] {0, 0, 0};
     bool _force_external;
     uint8_t _compass_instance;
     bool _initialized;
@@ -93,7 +92,7 @@ class AP_AK09916_BusDriver
 
     virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) = 0;
     virtual bool register_read(uint8_t reg, uint8_t *val) = 0;
-    virtual bool register_write(uint8_t reg, uint8_t val) = 0;
+    virtual bool register_write(uint8_t reg, uint8_t val, bool checked=false) = 0;
 
     virtual AP_HAL::Semaphore  *get_semaphore() = 0;
 
@@ -106,6 +105,14 @@ class AP_AK09916_BusDriver
 
     // return 24 bit bus identifier
     virtual uint32_t get_bus_id(void) const = 0;
+
+    /**
+     setup for register value checking. Frequency is how often to
+     check registers. If set to 10 then every 10th call to
+     check_next_register will check a register
+     */
+    virtual void setup_checked_registers(uint8_t num_regs) {}
+    virtual void check_next_register(void) {}
 };
 
 class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver
@@ -115,7 +122,7 @@ class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver
 
     virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override;
     virtual bool register_read(uint8_t reg, uint8_t *val) override;
-    virtual bool register_write(uint8_t reg, uint8_t val) override;
+    virtual bool register_write(uint8_t reg, uint8_t val, bool checked) override;
 
     virtual AP_HAL::Semaphore  *get_semaphore() override;
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
@@ -129,6 +136,18 @@ class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver
     uint32_t get_bus_id(void) const override {
         return _dev->get_bus_id();
     }
+
+    /**
+     setup for register value checking. Frequency is how often to
+     check registers. If set to 10 then every 10th call to
+     check_next_register will check a register
+     */
+    void setup_checked_registers(uint8_t num_regs) override {
+        _dev->setup_checked_registers(num_regs);
+    }
+    void check_next_register(void) override {
+        _dev->check_next_register();
+    }
 
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
@@ -143,7 +162,7 @@ class AP_AK09916_BusDriver_Auxiliary : public AP_AK09916_BusDriver
 
     bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override;
     bool register_read(uint8_t reg, uint8_t *val) override;
-    bool register_write(uint8_t reg, uint8_t val) override;
+    bool register_write(uint8_t reg, uint8_t val, bool checked) override;
 
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
 

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -120,6 +120,11 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
 void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,
                                            uint32_t max_samples)
 {
+    if (_compass._disable_mask & (1U<<instance)) {
+        // compass is disabled, discard data
+        return;
+    }
+
     /* rotate raw_field from sensor frame to body frame */
     rotate_field(field, instance);
 

libraries/AP_GPS/AP_GPS.cpp

@@ -962,14 +962,19 @@ void AP_GPS::update_primary(void)
     if (primary_instance == GPS_BLENDED_INSTANCE) {
         primary_instance = 0;
         for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
-            // choose GPS with highest state or higher number of satellites
+            // choose GPS with highest state or higher number of satellites with at least
+            if (state[i].status < GPS_OK_FIX_3D) {
+                continue;
+            }
             if ((state[i].status > state[primary_instance].status) ||
+                ((now - state[primary_instance].last_gps_time_ms) > 400 &&
+                 (now - state[i].last_gps_time_ms) < 400) ||
                 ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
                 primary_instance = i;
                 _last_instance_swap_ms = now;
-                gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS Switch: Switched from Blended to %u", primary_instance+1);
             }
         }
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS Switch: Switched from Blended to %u", primary_instance+1);
         return;
     }
 

libraries/AP_GPS/AP_GPS.h

@@ -213,9 +213,6 @@ class AP_GPS
 
     /// Query GPS status
     GPS_Status status(uint8_t instance) const {
-        if (_force_disable_gps && state[instance].status > NO_FIX) {
-            return NO_FIX;
-        }
         return state[instance].status;
     }
     GPS_Status status(void) const {
@@ -474,8 +471,8 @@ class AP_GPS
     bool logging_enabled(void) const { return _raw_data != 0; }
 
     // used to disable GPS for GPS failure testing in flight
-    void force_disable(bool disable) {
-        _force_disable_gps = disable;
+    void force_disable(uint8_t mask) {
+        _force_disable_mask = mask;
     }
 
     // handle possibly fragmented RTCM injection data
@@ -495,6 +492,11 @@ class AP_GPS
     // announce GPS version on mavlink
     void broadcast_gps_version(void);
 
+    // used to disable SBAS
+    void sbas_disable(uint8_t mask) {
+        _sbas_disable_mask = mask;
+    }
+
 protected:
 
     // configuration parameters
@@ -667,8 +669,12 @@ class AP_GPS
         GPS_AUTO_CONFIG_ENABLE  = 1
     };
 
-    // used for flight testing with GPS loss
-    bool _force_disable_gps;
+    // mask of GPS instances to disable. Used for flight testing with
+    // GPS loss
+    uint8_t _force_disable_mask;
+
+    // mask of SBAS instances to disable
+    uint8_t _sbas_disable_mask;
 
     // used to ensure we continue sending status messages if we ever detected the second GPS
     bool has_had_second_instance;

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -150,6 +150,9 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n
 
 void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
 {
+    if (is_disabled()) {
+        return;
+    }
     if (seen_fix2) {
         // use Fix2 instead
         return;
@@ -265,6 +268,9 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
 
 void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
 {
+    if (is_disabled()) {
+        return;
+    }
     bool process = false;
     seen_fix2 = true;
 
@@ -377,6 +383,9 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
 
 void AP_GPS_UAVCAN::handle_aux_msg(const AuxCb &cb)
 {
+    if (is_disabled()) {
+        return;
+    }
     WITH_SEMAPHORE(sem);
 
     if (!uavcan::isNaN(cb.msg->hdop)) {