From 281e9b974b65cf1185d15d84b16dc6ae31f47fbd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 13:19:06 +1100 Subject: [PATCH 1/9] AP_GPS: more flexible control of GPS disable options --- libraries/AP_GPS/AP_GPS.h | 12 +++++------- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 9 +++++++++ libraries/AP_GPS/AP_GPS_UBLOX.cpp | 10 +++++++++- libraries/AP_GPS/GPS_Backend.cpp | 7 +++++++ libraries/AP_GPS/GPS_Backend.h | 3 +++ 5 files changed, 33 insertions(+), 8 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index bfedfa07d9..b98f1487d8 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 @@ -667,8 +664,9 @@ 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; // 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..fe3e1df82c 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(); diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 81ef664f4e..1e8cac9492 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -289,3 +289,10 @@ 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< Date: Sun, 19 Dec 2021 13:28:46 +1100 Subject: [PATCH 2/9] AP_Compass: support set_disable_mask() --- libraries/AP_Compass/AP_Compass.h | 8 ++++++++ libraries/AP_Compass/AP_Compass_Backend.cpp | 5 +++++ 2 files changed, 13 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 259cf69d6b..69b0ac06f8 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/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 { 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< Date: Sun, 19 Dec 2021 13:52:54 +1100 Subject: [PATCH 3/9] AP_RangeFinder: added set_disable_mask() --- libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp | 6 ++++++ .../AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp | 4 ++++ libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp | 3 +++ libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp | 4 +++- libraries/AP_RangeFinder/RangeFinder.h | 8 ++++++++ 5 files changed, 24 insertions(+), 1 deletion(-) 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 Date: Sun, 19 Dec 2021 14:33:19 +1100 Subject: [PATCH 4/9] AP_IRLock: added way to disable sensor --- libraries/AP_IRLock/AP_IRLock_I2C.cpp | 4 ++++ libraries/AP_IRLock/AP_IRLock_SITL.cpp | 3 +++ libraries/AP_IRLock/IRLock.cpp | 9 +++++++++ libraries/AP_IRLock/IRLock.h | 21 ++++++++++++++++++++- 4 files changed, 36 insertions(+), 1 deletion(-) 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(); }; From f37cfbf4868d4d91909666c668a611bcb7a298c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 14:40:32 +1100 Subject: [PATCH 5/9] AP_HAL: added i2c_disable() --- libraries/AP_HAL/I2CDevice.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_HAL/I2CDevice.h b/libraries/AP_HAL/I2CDevice.h index 841e5575c1..051e7d9aa9 100644 --- a/libraries/AP_HAL/I2CDevice.h +++ b/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) {} }; /* From f7ad18e7c9bd55a605dd40b782f2c80b385c6bc1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 14:40:48 +1100 Subject: [PATCH 6/9] HAL_ChibiOS: added i2c_disable() support --- libraries/AP_HAL_ChibiOS/I2CDevice.cpp | 16 ++++++++++++++++ libraries/AP_HAL_ChibiOS/I2CDevice.h | 3 +++ 2 files changed, 19 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp index 84cbe5ac64..bec222a86e 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp +++ b/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 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; }; } From f7fa1cacb0961ccf2fe929ecacfac771cca70411 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 13:18:47 +1100 Subject: [PATCH 7/9] RC_Channel: added a full set of disable functions --- libraries/RC_Channel/RC_Channel.cpp | 67 ++++++++++++++++++++++++++++- libraries/RC_Channel/RC_Channel.h | 16 +++++++ 2 files changed, 82 insertions(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index f3346f65eb..939fd8cfcd 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -38,6 +38,8 @@ extern const AP_HAL::HAL& hal; #include #include #include +#include +#include #define SWITCH_DEBOUNCE_TIME_MS 200 @@ -475,6 +477,18 @@ 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: do_aux_function(ch_option, ch_flag); break; default: @@ -762,7 +776,58 @@ 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::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..48891cf073 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -181,6 +181,22 @@ 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, + // inputs eventually used to replace RCMAP MAINSAIL = 207, // mainsail input }; From 66b6141279fd970df589f10c2c97e49eb30fa101 Mon Sep 17 00:00:00 2001 From: rabad-matternet <67608224+rabad-matternet@users.noreply.github.com> Date: Tue, 15 Feb 2022 12:11:56 -0800 Subject: [PATCH 8/9] APM-138 Add SBAS disable option for test (#222) * APM-124: Change bootloader serial port to SERIAL_1 from SERIAL_5 The Flight Management Computer (FMC) connects to Ardupilot on SERIAL_1. This changes the booloader serial port from SERIAL_5 to SERIAL_1 so the FMC can update the Ardupilot's firmware using uploader.py. * APM-134: Secondary Mag health (#219) * APM-134: Secondary Mag health * AP_Compass: added register checking for AK09916 mode this allows AK09916 on I2C to recover from a power outage in flight * APM-138 Add SBAS disable option for test - added option to disable SBAS for GPS1, GPS2, or both - SBAS disabled via RC - add GNSS param defaults * AP_GPS: fixed switch to incorrect GPS from blended (#223) * AP_GPS: fixed switch to incorrect GPS from blended new GPS could have been about to time out * Bugfix/apm 144 gps2 low sats alert (#226) * AP_NavEKF2 AP_NavEKF3: Fix set alert for GPS1 and GPS2 in sat check Co-authored-by: zalice <48697569+zalice@users.noreply.github.com> * updated output to properly report SBAS status Co-authored-by: Dat Tran Co-authored-by: dat-tran-matternet <79175627+dat-tran-matternet@users.noreply.github.com> Co-authored-by: Andrew Tridgell Co-authored-by: zalice <48697569+zalice@users.noreply.github.com> Co-authored-by: Alice Zielman --- Tools/bootloaders/MttrCubeOrange_bl.bin | Bin 17372 -> 17420 bytes libraries/AP_Compass/AP_Compass.cpp | 12 ++++++++ libraries/AP_Compass/AP_Compass.h | 2 +- libraries/AP_Compass/AP_Compass_AK09916.cpp | 23 +++++++++------ libraries/AP_Compass/AP_Compass_AK09916.h | 27 +++++++++++++++--- libraries/AP_GPS/AP_GPS.cpp | 9 ++++-- libraries/AP_GPS/AP_GPS.h | 8 ++++++ libraries/AP_GPS/AP_GPS_UBLOX.cpp | 20 +++++++++++-- libraries/AP_GPS/GPS_Backend.cpp | 7 +++++ libraries/AP_GPS/GPS_Backend.h | 3 ++ .../hwdef/CubeOrange/defaults.parm | 4 +-- .../hwdef/CubeOrange/hwdef-bl.dat | 10 ++++--- .../hwdef/MttrCubeOrange/defaults.parm | 2 ++ .../AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 20 ++++++++++--- .../AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 20 ++++++++++--- libraries/GCS_MAVLink/GCS_Common.cpp | 5 ++++ libraries/RC_Channel/RC_Channel.cpp | 13 +++++++++ libraries/RC_Channel/RC_Channel.h | 4 +++ 18 files changed, 157 insertions(+), 32 deletions(-) diff --git a/Tools/bootloaders/MttrCubeOrange_bl.bin b/Tools/bootloaders/MttrCubeOrange_bl.bin index 69fd41369a14c419e1b98931055c8ff3e7d64e9a..1ae4bd4fdea51fa1b1705a9cc6ab64815fc440ef 100755 GIT binary patch delta 3625 zcmZu!4^&g<6~Euh3p|1e;eQBKUI_m|ngT*Zg%IMCzoJ!JrkxG7^aN`wb+%Pk7to%m zon52Ps)MO*)@-dPn(n93k z^OdTC#CDq32BBoDT9@rDRo6?Zhb{DM! z;e{zl*AHFv zYYo)*(PUSC14Z(LODYm>^RS&`uyF8$fSPyme zRIu?r4cHW|zqWv&nL1O}!i8^X=_Q)3lF<5(&8&9kz0lnsTY|fZVDp0Zw12HOT&of# zaSobW%Ou_5X3)M@3h(@adxV?K9p-}TSD3$c?LJOe5I&oPM?uKnam@ufNH+-cc0_me zjR5dxV=YL#kfjlq?(2!!`ME4eBGm{m7u&9;(>VOrBr zat}vqi7@;~f|cdy91uSB(3&7yP%8*FPez7gaA+9j{`L}N^js2`Vy30I6bN575_V$+ z2wffw7(}rwrnGkQY0nyb;DryJukL%+H> z#?p#YNrD9Cm9AW@GNtsd&R6^#(+Sr2{hU~iCgbYMacOhDQZ$q7*h}$(L=x2&Y+B)B z7Y6Y$CB>GX*+COsA|Fdogt`j41v+XmDwjB-@Tfy)|ebj%pQKt)d--&F(~$K>O^2*3-Ko#p*cN zA?|%-O5D4xBj_*)zjH^ODt48~X^qOt-TWVyIKRtwzsnbXmskBR?S2=Ne8^wO3;vn) z9oXG|y+MA}uRrV8uk8qGi@>gU$GwA_Np2w~m*if~1+|_Y7W?HlF0o&H6QvziY?u(M zV{wPm>JYoz z*Z%z?9&(B2-0E<&i)G~eLGHE_o6lJ?RW!a^_asN~_=;tX@PP2Sn>35p&&44rXTBw# z`!9zQxhq2%T;VOio$FncjcSH;-^96g@nTxj;HH{23&PS1C%W3n@yA_S;g5m9+(WEm zfayf~5cg5e3kIe;2$(}MU_M1S5#oFZ>P5N->CFdzf999ZstI3;cZE+kj?E~3L(ZoH z`#S&oS*ya;em&lSyS~%Ul_!`*18m5nKPBMmkySvVY$*{6rh=;2dYwhPWv4OlDiN}# zvMo{*$?h{|o9|t9;Wgifen$~L9|0napNBCp!+pGLE%i%8em{s35nkrI<&iqsd2IV7 zqP^&+?+L7=xfBa)B7$TprB~$oM)U?ED9-CK_R&;+LOxA4^Y<06<)giru)Zg%BC3+b zOF%W1%8Ar4HP;eea>Zq1FS}@cu|P>KASJ?IU5e5aah^XFlBljar|-Ly3GA~)z!;1n zU=wK?uVm-()gsXoP;FyW-U{$eU8FU zkkWux`1@i8rxBs}YvFVT?$E^lBrGw+0a+irM%cf&lo})Q{F$)z0j-#sA&rj@e?0HH zx1}!K)KVkA5qh;VHj7hnKIfn}pZmt5Gx?l<^8VA3Z#rJnVoBlF#I~4SeJU;uEe7~g z;mHe5d)J}wx*Ele@T(_o2QXROe4IPxHFCW%H!MEqFVWj*dgj00*eH!@ew(-K@`zm& z8L+<{#S_^GrZW}|OiZ*rOSPVoZ}c3kQ*|rjLF04Q+1u2AkjPY6VkIJ?$i8jPql9Gg zC*$I9{48$GY%H;~Jf@D>tg=v+6=t#ZscncBFQ%X)r8WJbA5@KnS+7xMO+Hb==m=aQi+y#CuB#> zGR{PcL+TqIiwhnZi;FPw#>6K{F5i^6g5=uoCMpTp%lneHlS+Pjas^I&mi)Cm7k68( z+9GyUc&`0yN*YlxCSc6-#UF(t>~~XxNd%e!Ze=CL@AC7s!EBeO=a5KIGEvor2rx*n zdtsq&pevVuBI5%}XS9dSy&fQ1Fg=nIpYtcveIty)=iKl?SFWAP@ep#z?w!|7$cKDq z-l+ImHtw8kCIRAHThc!xx-&y@Z;1q#~)D^YG=xEraHm~BX3u6P9mW6YnmJ2qd#XUco;9DuTi%%@B?BN+r% z*{5_7KE|MzVukUXKVitX)^0Uv6Hz_A!$v^HD77=U~opGiz1k0V_fb^Hd%`#~7cm7kk!% zDzTkK^>Y2ps&&t9s6`Q=Xz=KW*3)8b8_>2c zT?T!g*2=M3(HXEB;bdsVnu2y_>il5~cAO)&wsx^`>GFcqgoKd!UeIaho%6fj{nq>4 z?|%0ZUf)H&+DS5j0_Hsl(3wi0XChvCKp#iij`BaEcW-8T#LxLa#}xowir9oWwD(Ys z!m3qntwwY8+o7@XwU&uf+$0nX7e9N^|Fu0iSQ@>o-{p{^D*7w+N90(o7*?v z+$<4-JQODp^7z?Qg2Fc%)(yf3ZV+Dgf$##qiqhoY_8xIE(pp1p5p9sYo4ZY}md*6RcX-Iu`aH;PpW#j0fBt`B>zJ8*-T^>mLkUdrG3 zQuLZXzMX^A9|ojtweeIc3}0{g<`xLj@QBH=pz4rmDGlqMl@CI*XDL@D0R7kvO4C$! z-$7|!EV~yN1&3v-j^U_TARP5jn$XtJ4iFkV85tvkHyx1q;Q(ax42VkwkIQf=5Kgxc zW_=|HyFC~%M8Ytb(p>M;p52~m4um&7!CY;3H5cL}At1c%fmG*U2G_|!$|~dA4x%mK zB#ah4rh310(^>Btf-YxS(5>D>vhEI2b4DDHENScHGPwB0x!hvo3KIzV-so<~Q1wBI z-*w5i9Fr;>*>D;%3P;wRmN1ew@g!ZIY%K86(Jm**`-fMws0FEv6`p7bx+|CBLJV(! z;Uz6+e`~hDa8=8)(y(h2wcI^QwOXh)NbesmX`$NYSG>QJ%>LDJ5%=5bo7k_uC1HBR z)qgq}O8I`WAcdUrrKPHp)V`%$A|~GFN#y*R>17~%=c&4LLY`ip#XarWGwgT0<)Z@i zQezT(f@Iu5hgijWAEXwJgrD}io(;@GPFGn3kT2oHEcca8y?oZ0poqMxJV>$?18IU( zqUw~FEOs*)q5D)El^EJ7cT#c?YQ0kKrlXpZ@*rFsm9~Yp{_0S5h^JkNr~TUKYKM4D z_a_1Kd-DU+in3h1Rh3Aq5%Y1rUuOYE)dAYo4_am3D5P3OKDLV4Dgrs4v4$Q1;UA-_ z_lwVeUnm(UEgbpQ%5t=mqCuEFN@?V+a%}H0w=-~;{I0`jZz&upv-(|K0oT)3 zWgE$vG(aaRI{_=u6ObOWitmFGU7e$T*BfJV@#c?vv_Fcim}V7QF~$1FiX3o@*U~aF zHIUSXK#UlxwAJt0I94>gE31Yh!q*>2%qB#ijReGQ^xK7QM{-An@ODSW(K$R zN5GxWb5pHillt#}e@c8vT1KfhOa*u zT04K2Zw=E~L<8)mBd-#0_sAZ71v=?`v?}@35 zS;*i`&KP@y6Y0{iOmkG(6?ZFh*-hz-1&Q7#><3;Ho*Nkx>oSm>8VnXo)fe0)SK zNw&?2P!Jg|-IUB%M9$JJX1=edjoLL5{M({rL0EA{%{cITh!3*?qa(x8|%8 zzw3AeRAz~JiAilz z)e)_9%ed?By(iQ7HOyQJEiUozF*)(C^+MlCgTdd_6ytbVQ-xz<;&V72Ox%d0Tw5SJgPpLRU#OiX8Taw)wF^;vR{NOz7Unn! zRX<@2-Q4&Y!05#GLrE!O38Jxw;%F|68+X0HZ`4J{F2~3B-XJDm2gtnbu99(+P`8F} z)0Hc1_N3|42ZIxfxKxzs_QvSV^N>u!k@=aJa?$)Sk!p`c8%C=ogO(Wq~@pqqTw)lHbge@{H zluSl5z$Re~Kz9Y^6q{k2nLdj|i(HAMmYoc&6nhosXZ3ew^Ur6Tm5deH=45^uBwElt z(lWp6rE&W$n)SQZpY6)FCC~B@vcsmC(@n?;{)@SH#ADI(=k>98SBLa3V+B{tyH8B~ zvfLTuyzQ0Tt%Ur*`}4M0YK%4IYEz9d&UDaJQ*q4%iR!wl5!P_9?qEYJHFia9HjFfH zR+|?GgS%A zMUR`FSZk=bw$f0ct_wuVR-5v%NA^j_t`yip^5+qHwY?djwm*rlDA3W5K4hLnM*omG zicI1|<}+j_J!CuuE967xpzlG5m4)jq@1C1rfC#LD_y?Y($PJ$~0tpU?G-+vh6v`drg+{;SvT%JWTB-}@1OvWLu* zl(ZViATUb1(oOhsetup_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_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 7b9712388a..aee1b56ffa 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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 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 b98f1487d8..310ee7dad0 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -492,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 @@ -668,6 +673,9 @@ class AP_GPS // 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_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index fe3e1df82c..6cc6c59cc7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -983,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) { @@ -996,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 1e8cac9492..eb40f5ad22 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -296,3 +296,10 @@ bool AP_GPS_Backend::is_disabled(void) const const uint8_t instance = &state - &gps.state[0]; return ((1U<_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/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 58a377ce99..6ba50ba361 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4167,6 +4167,11 @@ void GCS_MAVLINK::send_sys_status() errors2, 0, // errors3 errors4); // errors4 + + const Compass &compass = AP::compass(); + if (compass.enabled() && ((control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG) == 0)) { + gcs().send_named_int("COMPHEALTH", int(compass.get_healthy_mask())); + } } void GCS_MAVLINK::send_extended_sys_state() const diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 939fd8cfcd..8385a68903 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -489,6 +489,9 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const aux_switch_ 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: do_aux_function(ch_option, ch_flag); break; default: @@ -789,6 +792,16 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po 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::MAG_DISABLE1: AP::compass().set_disable_mask(ch_flag == HIGH?1:0); break; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 48891cf073..5b9b2c3ad1 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -197,6 +197,10 @@ class RC_Channel { PLAND_DISABLE = 160, I2C_DISABLE = 161, + GPS_SBAS_DISABLE1 = 162, + GPS_SBAS_DISABLE2 = 163, + GPS_SBAS_DISABLE_BOTH = 164, + // inputs eventually used to replace RCMAP MAINSAIL = 207, // mainsail input }; From 59dd4f29fcc7bacfdc79db52e93bd8054677ea7f Mon Sep 17 00:00:00 2001 From: Dat Tran Date: Mon, 14 Feb 2022 10:21:38 -0600 Subject: [PATCH 9/9] APM-145: Add motor kill option for test --- ArduCopter/motors.cpp | 20 ++++++++++++++++++++ libraries/RC_Channel/RC_Channel.cpp | 23 +++++++++++++++++++++++ libraries/RC_Channel/RC_Channel.h | 6 ++++++ 3 files changed, 49 insertions(+) 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/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 8385a68903..77e6739692 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -41,6 +41,8 @@ extern const AP_HAL::HAL& hal; #include #include +void set_motor_kill(uint8_t motor_num, bool kill); + #define SWITCH_DEBOUNCE_TIME_MS 200 const AP_Param::GroupInfo RC_Channel::var_info[] = { @@ -492,6 +494,11 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const aux_switch_ 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: @@ -802,6 +809,22 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po 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; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 5b9b2c3ad1..be15059910 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -201,6 +201,12 @@ class RC_Channel { 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 };