Chore/Add RC failure options for mttr-2.2.1

ArduCopter/motors.cpp

@@ -4,9 +4,14 @@
 #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
+#define UINT_8BIT_MAX           255
 
 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 +181,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 == UINT_8BIT_MAX) {
+        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.h

@@ -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_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.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
@@ -684,8 +686,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)) {

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -567,8 +567,16 @@ AP_GPS_UBLOX::read(void)
     }
 
     numc = port->available();
-    for (int16_t i = 0; i < numc; i++) {        // Process bytes received
 
+    if (is_disabled()) {
+        // drain data
+        for (int16_t i = 0; i < numc; i++) {
+            data = port->read();
+        }
+        return false;
+    }
+
+    for (int16_t i = 0; i < numc; i++) {        // Process bytes received
         // read the next byte
         data = port->read();
 
@@ -975,6 +983,11 @@ AP_GPS_UBLOX::_parse_gps(void)
                     (unsigned)_buffer.gnss.configBlock[i].flags);
                 }
 #endif
+                if (_buffer.gnss.configBlock[1].flags & 0x1) {
+                    // GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "SBAS Enabled!\n");
+                } else {
+                    GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "SBAS Disabled!\n");
+                }
 
                 for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
                     if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) {
@@ -988,11 +1001,20 @@ AP_GPS_UBLOX::_parse_gps(void)
                         if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
                             _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
                             _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
+                            _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
                         } else {
-                            _buffer.gnss.configBlock[i].resTrkCh = 1;
-                            _buffer.gnss.configBlock[i].maxTrkCh = 3;
+                            // SBAS configuration
+                            _buffer.gnss.configBlock[i].flags &= 0xFF000000; // this will disable SBAS
+                            if (is_sbas_disabled()) {
+                                // nothing needs to be done
+                            }
+                            else {
+                                // bit  0, 1 = enable
+                                // bit 16, 1 = SBAS L1 C/A
+                                _buffer.gnss.configBlock[i].flags |= (1U<<16);
+                                _buffer.gnss.configBlock[i].flags |= 0x00000001;
+                            }
                         }
-                        _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
                     } else {
                         _buffer.gnss.configBlock[i].resTrkCh = 0;
                         _buffer.gnss.configBlock[i].maxTrkCh = 0;

libraries/AP_GPS/GPS_Backend.cpp

@@ -289,3 +289,17 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
         state.uart_timestamp_ms = local_us / 1000U;
     }
 }
+
+// check if an instance is disabled
+bool AP_GPS_Backend::is_disabled(void) const
+{
+    const uint8_t instance = &state - &gps.state[0];
+    return ((1U<<instance) & gps._force_disable_mask) != 0;
+}
+
+// check if an instance has SBAS in disabled
+bool AP_GPS_Backend::is_sbas_disabled(void) const
+{
+    const uint8_t instance = &state - &gps.state[0];
+    return ((1U<<instance) & gps._sbas_disable_mask) != 0;
+}

libraries/AP_GPS/GPS_Backend.h

@@ -114,6 +114,12 @@ class AP_GPS_Backend
         return uint16_t(gps._driver_options.get());
     }
 
+    // check if an instance is disabled
+    bool is_disabled(void) const;
+
+    // check if an instance has SBAS in disabled
+    bool is_sbas_disabled(void) const;
+
 private:
     // itow from previous message
     uint32_t _last_itow;

libraries/AP_HAL/I2CDevice.h

@@ -104,6 +104,9 @@ class I2CDeviceManager {
       get mask of bus numbers for all configured internal I2C buses
      */
     virtual uint32_t get_bus_mask_internal(void) const { return 0x01; }
+
+    // disable i2c devices for testing
+    virtual void i2c_disable(bool disable) {}
 };
 
 /*

libraries/AP_HAL_ChibiOS/I2CDevice.cpp

@@ -64,6 +64,9 @@ I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(I2CD)];
 #define HAL_I2C_CLEAR_ON_TIMEOUT 1
 #endif
 
+// allow disable of all i2c for testing
+static bool _i2c_disable;
+
 // get a handle for DMA sharing DMA channels with other subsystems
 void I2CBus::dma_init(void)
 {
@@ -208,6 +211,10 @@ void I2CBus::dma_deallocate(Shared_DMA *)
 bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
+    if (_i2c_disable) {
+        return false;
+    }
+
     if (!bus.semaphore.check_owner()) {
         hal.console->printf("I2C: not owner of 0x%x for addr 0x%02x\n", (unsigned)get_bus_id(), _address);
         return false;
@@ -256,6 +263,9 @@ bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
 bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
+    if (_i2c_disable) {
+        return false;
+    }
     i2cAcquireBus(I2CD[bus.busnum].i2c);
 
     if (!bus.bouncebuffer_setup(send, send_len, recv, recv_len)) {
@@ -383,4 +393,10 @@ uint32_t I2CDeviceManager::get_bus_mask_external(void) const
     return get_bus_mask() & ~HAL_I2C_INTERNAL_MASK;
 }
 
+// disable i2c devices for testing
+void I2CDeviceManager::i2c_disable(bool disable)
+{
+    _i2c_disable = disable;
+}
+
 #endif // HAL_USE_I2C

libraries/AP_HAL_ChibiOS/I2CDevice.h

@@ -143,6 +143,9 @@ class I2CDeviceManager : public AP_HAL::I2CDeviceManager {
       get mask of bus numbers for all configured internal I2C buses
      */
     uint32_t get_bus_mask_internal(void) const override;
+
+    // disable i2c devices for testing
+    void i2c_disable(bool disable) override;
 };
 }
 

libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm

@@ -86,6 +86,8 @@ FS_GCS_ENABLE	0
 FS_THR_ENABLE	6
 FS_THR_VALUE	0
 GND_EFFECT_COMP	1
+GPS_GNSS_MODE   127
+GPS_GNSS_MODE2  127
 GPS_HDOP_GOOD	175
 GPS_POS1_X	0.182
 GPS_POS1_Y	-0.05

libraries/AP_IRLock/AP_IRLock_I2C.cpp

@@ -162,6 +162,10 @@ void AP_IRLock_I2C::read_frames(void)
         return;
     }
 
+    if (_disabled) {
+        return;
+    }
+
     // The objects in each frame are sorted by size, with the largest objects sent first.
     struct frame *irframe = &blocks[0];
     int16_t corner1_pix_x = irframe->pixel_x - irframe->pixel_size_x/2;

libraries/AP_IRLock/AP_IRLock_SITL.cpp

@@ -34,6 +34,9 @@ void AP_IRLock_SITL::init(int8_t bus)
 // retrieve latest sensor data - returns true if new data is available
 bool AP_IRLock_SITL::update()
 {
+    if (_disabled) {
+        return false;
+    }
     // return immediately if not healthy
     _flags.healthy = _sitl->precland_sim.healthy();
     if (!_flags.healthy) {

libraries/AP_IRLock/IRLock.cpp

@@ -7,6 +7,8 @@
 
 #include "IRLock.h"
 
+IRLock *IRLock::singleton;
+
 // retrieve body frame unit vector in direction of target
 // returns true if data is available
 bool IRLock::get_unit_vector_body(Vector3f& ret) const
@@ -23,3 +25,10 @@ bool IRLock::get_unit_vector_body(Vector3f& ret) const
     ret /= ret.length();
     return true;
 }
+
+namespace AP {
+IRLock *irlock()
+{
+    return IRLock::get_singleton();
+}
+}

libraries/AP_IRLock/IRLock.h

@@ -26,6 +26,10 @@
 class IRLock
 {
 public:
+    IRLock(void) {
+        singleton = this;
+    }
+
     // init - initialize sensor library
     // library won't be useable unless this is first called
     virtual void init(int8_t bus) = 0;
@@ -45,7 +49,14 @@ class IRLock
     // retrieve body frame unit vector in direction of target
     // returns true if data is available
     bool get_unit_vector_body(Vector3f& ret) const;
-
+
+    void set_disable(bool disable) {
+        _disabled = disable;
+    }
+
+    static IRLock *get_singleton(void) {
+        return singleton;
+    }
 
 protected:
     struct AP_IRLock_Flags {
@@ -64,4 +75,12 @@ class IRLock
     } irlock_target_info;
 
     irlock_target_info _target_info;
+
+    bool _disabled;
+
+    static IRLock *singleton;
+};
+
+namespace AP {
+    IRLock *irlock();
 };