From d0bcc766c95c072073c24a9ab11251ffb49284f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 13:19:06 +1100 Subject: [PATCH 01/13] 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 3771948896..cc489b8942 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 @@ -684,8 +681,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 706ce20149..1afdc59ca2 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 02/13] 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 e4d02d1605..8b80ce133c 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 03/13] cherry-pick d846a38, fix merge conflicts --- .../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 ++++++++ 4 files changed, 18 insertions(+), 1 deletion(-) 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 fd402e2e0c..97a9e1018c 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -106,6 +106,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]; @@ -170,6 +171,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 04/13] 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 2a91b97b3e1489fdffb926eaa63f402080c72e4a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 14:40:32 +1100 Subject: [PATCH 05/13] 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 ce7a19143d2197166fce8eac13a5f3670239ba21 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 14:40:48 +1100 Subject: [PATCH 06/13] 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 918a3322e51dd94a7911713ce4f1e4a50ac6a86b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 Dec 2021 13:18:47 +1100 Subject: [PATCH 07/13] 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 09db54f76a3510343ff9e31d30754463c59aad29 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 08/13] cherry-pick 66b6141, fix merge conflicts --- 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/MttrCubeOrange/defaults.parm | 2 ++ libraries/RC_Channel/RC_Channel.cpp | 13 ++++++++++++ libraries/RC_Channel/RC_Channel.h | 4 ++++ 7 files changed, 54 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index cc489b8942..d580c40223 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 @@ -685,6 +690,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 1afdc59ca2..287b6445dd 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< Date: Mon, 14 Feb 2022 10:21:38 -0600 Subject: [PATCH 09/13] 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 }; From 1278b3e8378435d3e06d3ca14e56aeef3a42c894 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adrienne=20Ti=C3=B1a?= Date: Mon, 15 Sep 2025 12:51:16 -0700 Subject: [PATCH 10/13] fix: add missing state disabled check for lidar rc failure change --- libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index 8ed510ccd7..a5b2a9a536 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -445,6 +445,9 @@ void AP_RangeFinder_LightWareI2C::update(void) void AP_RangeFinder_LightWareI2C::sf20_timer(void) { + if (state.disabled) { + return; + } #ifdef MFG_TEST_BUILD static const uint32_t TIMER_FREQ_HZ = 20; static uint32_t count = 0; From 460a79aaaba638f4e35b498884eae31b122c27ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adrienne=20Ti=C3=B1a?= Date: Mon, 15 Sep 2025 12:54:07 -0700 Subject: [PATCH 11/13] chore: disable noisy sbas enabled alert --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 287b6445dd..96deebf709 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -984,7 +984,7 @@ AP_GPS_UBLOX::_parse_gps(void) } #endif if (_buffer.gnss.configBlock[1].flags & 0x1) { - GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "SBAS Enabled!\n"); + // GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "SBAS Enabled!\n"); } else { GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "SBAS Disabled!\n"); } From 08165f980271e63d706ff358acea1930cba1e56b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adrienne=20Ti=C3=B1a?= Date: Mon, 15 Sep 2025 14:17:40 -0700 Subject: [PATCH 12/13] Fix: add variable name for motor kill motor number, replace magic number --- ArduCopter/motors.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 818f680c2f..14188410e4 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -4,6 +4,7 @@ #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 255 static uint32_t auto_disarm_begin; @@ -193,7 +194,7 @@ void Copter::motors_output() void set_motor_kill(uint8_t motor_num, bool kill) { - if (motor_num == 0xFF) { + if (motor_num == UINT_8BIT_MAX) { motor_kill_all = kill; } else if (motor_num < MOTOR_COUNT) { motor_kill[motor_num] = kill; From a3c11e24c0be4313f8d701e197540efc73b9a957 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adrienne=20Ti=C3=B1a?= Date: Mon, 15 Sep 2025 14:24:48 -0700 Subject: [PATCH 13/13] fix: typo from previous commit --- ArduCopter/motors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 14188410e4..eff6c47f75 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -4,7 +4,7 @@ #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 255 +#define UINT_8BIT_MAX 255 static uint32_t auto_disarm_begin;