diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 4b8bc879cc..818f680c2f 100644 --- a/ArduCopter/motors.cpp +++ b/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() { diff --git a/Tools/bootloaders/MttrCubeOrange_bl.bin b/Tools/bootloaders/MttrCubeOrange_bl.bin index 69fd41369a..1ae4bd4fde 100755 Binary files a/Tools/bootloaders/MttrCubeOrange_bl.bin and b/Tools/bootloaders/MttrCubeOrange_bl.bin differ diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 6f13ec2ad1..970a42cf5f 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/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) { diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 259cf69d6b..8b80ce133c 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/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 { diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 0a9ce2545e..9c64e36299 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/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 *) ®s, 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; } diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index 9bcd6cf549..cbe93de269 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/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 _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; diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index cba5958283..245d21930a 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/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< 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; } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index bfedfa07d9..310ee7dad0 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index d1d5c18e28..bfd8fb235f 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/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)) { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index b6a0457abd..6cc6c59cc7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 81ef664f4e..eb40f5ad22 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/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<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 diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h index 72580d7a88..21502cf714 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.h +++ b/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; }; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/defaults.parm index 357bba7d85..c56e6f094d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/defaults.parm @@ -16,8 +16,8 @@ BRD_IMUHEAT_P 50 # compass primary defaults COMPASS_PRIO1_ID 73475 -COMPASS_PRIO2_ID 592905 -COMPASS_PRIO2_ID 0 +COMPASS_PRIO2_ID 1313801 +COMPASS_PRIO3_ID 0 # Failsafe primary defaults FS_OPTIONS 23 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.dat index 03b2332322..779f9f8bbf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.dat @@ -29,11 +29,13 @@ FLASH_BOOTLOADER_LOAD_KB 128 define HAL_LED_ON 0 # order of UARTs (and USB) -SERIAL_ORDER OTG1 UART7 +SERIAL_ORDER OTG1 USART2 -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). -PE7 UART7_RX UART7 -PE8 UART7_TX UART7 +# UART2 maps to serial1 in SERIALn_ parameters +PD3 USART2_CTS USART2 +PD4 USART2_RTS USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 # Pin for PWM Voltage Selection PB4 PWM_VOLT_SEL OUTPUT HIGH diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm index 7c0292a1b0..2eb8b18d6c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm @@ -81,6 +81,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 diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.cpp b/libraries/AP_IRLock/AP_IRLock_I2C.cpp index f42018ef12..e00822dc39 100644 --- a/libraries/AP_IRLock/AP_IRLock_I2C.cpp +++ b/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; diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.cpp b/libraries/AP_IRLock/AP_IRLock_SITL.cpp index f54c483d97..3ec33a0197 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.cpp +++ b/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) { diff --git a/libraries/AP_IRLock/IRLock.cpp b/libraries/AP_IRLock/IRLock.cpp index 1308e3fac4..57dd7d919d 100644 --- a/libraries/AP_IRLock/IRLock.cpp +++ b/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(); +} +} diff --git a/libraries/AP_IRLock/IRLock.h b/libraries/AP_IRLock/IRLock.h index 28dcb82b12..2bfc13e795 100644 --- a/libraries/AP_IRLock/IRLock.h +++ b/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(); }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index dff1d49f4b..e1f7fe80c9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -201,17 +201,29 @@ void NavEKF2_core::calcGpsGoodToAlign(void) bool numSatsGPS1Fail = (gps.num_sats(0) < gps.num_sats_arm_min()) && (frontend->_gpsCheck & MASK_GPS_NSATS); // fail if GPS2 is enabled and not enough sats for secondary GPS2 - bool numSatsGPS2Fail = (gps.get_type(1) == 1) && (gps.num_sats(1) < gps.num_sats_arm_min()) && (frontend->_gpsCheck & MASK_GPS_NSATS); + bool gps2Present = gps.num_sensors() > 1; + bool numSatsGPS2Fail = (gps2Present) && (gps.num_sats(1) < gps.num_sats_arm_min()) && (frontend->_gpsCheck & MASK_GPS_NSATS); // Report check result as a text string and bitmask if (numSatsGPS1Fail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS 1 numsats %u (needs %u)", gps.num_sats(0), gps.num_sats_arm_min()); + "GPS1 numsats %u (needs %u)", gps.num_sats(0), gps.num_sats_arm_min()); gpsCheckStatus.bad_sats = true; } if (numSatsGPS2Fail) { - hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS 2 numsats %u (needs %u)", gps.num_sats(1), gps.num_sats_arm_min()); + if (gps2Present) { + if (numSatsGPS1Fail) { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), + "GPS1 numsats %u GPS2 %u (need %u)", gps.num_sats(0), gps.num_sats(1), gps.num_sats_arm_min()); + } + else { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), + "GPS2 numsats %u (needs %u)", gps.num_sats(1), gps.num_sats_arm_min()); + } + } + else { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS2 not present for sat check"); + } gpsCheckStatus.bad_sats = true; } if (!numSatsGPS1Fail && !numSatsGPS2Fail) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 388b482cca..0324084936 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -198,17 +198,29 @@ void NavEKF3_core::calcGpsGoodToAlign(void) bool numSatsGPS1Fail = (gps.num_sats(0) < gps.num_sats_arm_min()) && (frontend->_gpsCheck & MASK_GPS_NSATS); // fail if GPS2 is enabled and not enough sats for secondary GPS2 - bool numSatsGPS2Fail = (gps.get_type(1) == 1) && (gps.num_sats(1) < gps.num_sats_arm_min()) && (frontend->_gpsCheck & MASK_GPS_NSATS); + bool gps2Present = gps.num_sensors() > 1; + bool numSatsGPS2Fail = (gps2Present) && (gps.num_sats(1) < gps.num_sats_arm_min()) && (frontend->_gpsCheck & MASK_GPS_NSATS); // Report check result as a text string and bitmask if (numSatsGPS1Fail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS 1 numsats %u (needs %u)", gps.num_sats(0), gps.num_sats_arm_min()); + "GPS1 numsats %u (needs %u)", gps.num_sats(0), gps.num_sats_arm_min()); gpsCheckStatus.bad_sats = true; } if (numSatsGPS2Fail) { - hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS 2 numsats %u (needs %u)", gps.num_sats(1), gps.num_sats_arm_min()); + if (gps2Present) { + if (numSatsGPS1Fail) { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), + "GPS1 numsats %u GPS2 %u (need %u)", gps.num_sats(0), gps.num_sats(1), gps.num_sats_arm_min()); + } + else { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), + "GPS2 numsats %u (needs %u)", gps.num_sats(1), gps.num_sats_arm_min()); + } + } + else { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS2 not present for sat check"); + } gpsCheckStatus.bad_sats = true; } if (!numSatsGPS1Fail && !numSatsGPS2Fail) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index 71656406ab..35fc0212ce 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -467,6 +467,9 @@ void AP_RangeFinder_LightWareI2C::update(void) void AP_RangeFinder_LightWareI2C::legacy_timer(void) { + if (state.disabled) { + return; + } if (legacy_get_reading(state.distance_cm)) { // update range_valid state based on distance measured update_status(); @@ -477,6 +480,9 @@ void AP_RangeFinder_LightWareI2C::legacy_timer(void) void AP_RangeFinder_LightWareI2C::sf20_timer(void) { + if (state.disabled) { + return; + } if (sf20_get_reading(state.distance_cm)) { // update range_valid state based on distance measured update_status(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index de0ae939a9..272d85878c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -65,6 +65,10 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm) int16_t nbytes = uart->available(); while (nbytes-- > 0) { char c = uart->read(); + if (state.disabled) { + continue; + } + if (c == '\r') { linebuf[linebuf_len] = 0; const float dist = (float)atof(linebuf); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp index 2ed6b3490a..c4ca107b5c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp @@ -129,6 +129,9 @@ void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t nod if (driver == nullptr) { return; } + if (driver->state.disabled) { + return; + } WITH_SEMAPHORE(driver->_sem); switch (cb.msg->reading_type) { case uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE: diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp index b8ab474630..711032c3df 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp @@ -74,7 +74,9 @@ bool AP_RangeFinder_uLanding::detect_version(void) while (nbytes-- > 0) { uint8_t c = uart->read(); - + if (state.disabled) { + continue; + } if (((c == ULANDING_HDR_V0) || (c == ULANDING_HDR)) && !hdr_found) { byte1 = c; hdr_found = true; diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 0eff84d4e9..ce16396047 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -103,6 +103,7 @@ class RangeFinder uint32_t last_reading_ms; // system time of last successful update from sensor const struct AP_Param::GroupInfo *var_info; + bool disabled; }; static const struct AP_Param::GroupInfo *backend_var_info[RANGEFINDER_MAX_INSTANCES]; @@ -167,6 +168,13 @@ class RangeFinder static RangeFinder *get_singleton(void) { return _singleton; } + // allow disable of set of rangefinders + void set_disable_mask(uint8_t mask) { + for (uint8_t i=0; i #include #include +#include +#include + +void set_motor_kill(uint8_t motor_num, bool kill); #define SWITCH_DEBOUNCE_TIME_MS 200 @@ -475,6 +479,26 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const aux_switch_ case AUX_FUNC::GPS_DISABLE: case AUX_FUNC::KILL_IMU1: case AUX_FUNC::KILL_IMU2: + case AUX_FUNC::GPS_DISABLE1: + case AUX_FUNC::GPS_DISABLE2: + case AUX_FUNC::GPS_DISABLE_BOTH: + case AUX_FUNC::MAG_DISABLE1: + case AUX_FUNC::MAG_DISABLE2: + case AUX_FUNC::MAG_DISABLE_BOTH: + case AUX_FUNC::RFND_DISABLE1: + case AUX_FUNC::RFND_DISABLE2: + case AUX_FUNC::RFND_DISABLE_BOTH: + case AUX_FUNC::PIXY_DISABLE: + case AUX_FUNC::I2C_DISABLE: + case AUX_FUNC::PLAND_DISABLE: + case AUX_FUNC::GPS_SBAS_DISABLE1: + case AUX_FUNC::GPS_SBAS_DISABLE2: + case AUX_FUNC::GPS_SBAS_DISABLE_BOTH: + case AUX_FUNC::MOTOR_KILL_ALL: + case AUX_FUNC::MOTOR_KILL_1: + case AUX_FUNC::MOTOR_KILL_2: + case AUX_FUNC::MOTOR_KILL_3: + case AUX_FUNC::MOTOR_KILL_4: do_aux_function(ch_option, ch_flag); break; default: @@ -762,7 +786,84 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po } case AUX_FUNC::GPS_DISABLE: - AP::gps().force_disable(ch_flag == HIGH); + AP::gps().force_disable(ch_flag == HIGH?1:0); + break; + + case AUX_FUNC::GPS_DISABLE1: + AP::gps().force_disable(ch_flag == HIGH?1:0); + break; + case AUX_FUNC::GPS_DISABLE2: + AP::gps().force_disable(ch_flag == HIGH?2:0); + break; + case AUX_FUNC::GPS_DISABLE_BOTH: + AP::gps().force_disable(ch_flag == HIGH?3:0); + break; + + case AUX_FUNC::GPS_SBAS_DISABLE1: + AP::gps().sbas_disable(ch_flag == HIGH?1:0); + break; + case AUX_FUNC::GPS_SBAS_DISABLE2: + AP::gps().sbas_disable(ch_flag == HIGH?2:0); + break; + case AUX_FUNC::GPS_SBAS_DISABLE_BOTH: + AP::gps().sbas_disable(ch_flag == HIGH?3:0); + break; + + case AUX_FUNC::MOTOR_KILL_ALL: + set_motor_kill(0xFF, ch_flag == HIGH); + break; + case AUX_FUNC::MOTOR_KILL_1: + set_motor_kill(0, ch_flag == HIGH); + break; + case AUX_FUNC::MOTOR_KILL_2: + set_motor_kill(1, ch_flag == HIGH); + break; + case AUX_FUNC::MOTOR_KILL_3: + set_motor_kill(2, ch_flag == HIGH); + break; + case AUX_FUNC::MOTOR_KILL_4: + set_motor_kill(3, ch_flag == HIGH); + break; + + case AUX_FUNC::MAG_DISABLE1: + AP::compass().set_disable_mask(ch_flag == HIGH?1:0); + break; + case AUX_FUNC::MAG_DISABLE2: + AP::compass().set_disable_mask(ch_flag == HIGH?2:0); + break; + case AUX_FUNC::MAG_DISABLE_BOTH: + AP::compass().set_disable_mask(ch_flag == HIGH?3:0); + break; + + case AUX_FUNC::RFND_DISABLE1: + AP::rangefinder()->set_disable_mask(ch_flag == HIGH?1:0); + break; + case AUX_FUNC::RFND_DISABLE2: + AP::rangefinder()->set_disable_mask(ch_flag == HIGH?2:0); + break; + case AUX_FUNC::RFND_DISABLE_BOTH: + AP::rangefinder()->set_disable_mask(ch_flag == HIGH?3:0); + break; + + case AUX_FUNC::PIXY_DISABLE: { + auto *irlock = AP::irlock(); + if (irlock) { + irlock->set_disable(ch_flag == HIGH); + } + break; + } + + case AUX_FUNC::PLAND_DISABLE: { + auto *irlock = AP::irlock(); + if (irlock) { + irlock->set_disable(ch_flag == HIGH); + } + AP::rangefinder()->set_disable_mask(ch_flag == HIGH?3:0); + break; + } + + case AUX_FUNC::I2C_DISABLE: + hal.i2c_mgr->i2c_disable(ch_flag == HIGH); break; case AUX_FUNC::MOTOR_ESTOP: diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index a226034228..be15059910 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -181,6 +181,32 @@ class RC_Channel { // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp! // also, if you add an option >255, you will need to fix duplicate_options_exist + // custom sensor disable options + GPS_DISABLE1 = 150, + GPS_DISABLE2 = 151, + GPS_DISABLE_BOTH = 152, + + MAG_DISABLE1 = 153, + MAG_DISABLE2 = 154, + MAG_DISABLE_BOTH = 155, + + RFND_DISABLE1 = 156, + RFND_DISABLE2 = 157, + RFND_DISABLE_BOTH = 158, + PIXY_DISABLE = 159, + PLAND_DISABLE = 160, + I2C_DISABLE = 161, + + GPS_SBAS_DISABLE1 = 162, + GPS_SBAS_DISABLE2 = 163, + GPS_SBAS_DISABLE_BOTH = 164, + + MOTOR_KILL_ALL = 165, + MOTOR_KILL_1 = 166, + MOTOR_KILL_2 = 167, + MOTOR_KILL_3 = 168, + MOTOR_KILL_4 = 169, + // inputs eventually used to replace RCMAP MAINSAIL = 207, // mainsail input };