From 53ee7d6e75a09714594a8f9d6665bba5987b5163 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 07:28:06 +1100 Subject: [PATCH 01/47] AP_InertialSensor: fixed check for changes to notch filters if the configured freq changes on any type of notch then A and Q change, so init must be called. This does not affect only Fixed notches --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 8c6f05fd1e..0d8e6029e8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1863,8 +1863,7 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv { if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) || !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) || - (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && - !is_equal(last_center_freq_hz[instance], params.center_freq_hz())) || + !is_equal(last_center_freq_hz[instance], params.center_freq_hz()) || converging) { filter[instance].init(gyro_rate, params); last_center_freq_hz[instance] = params.center_freq_hz(); From 6efeef9cec424b86c20fb9f7a29fec66f7ec720d Mon Sep 17 00:00:00 2001 From: Baris Date: Mon, 2 Dec 2024 15:17:15 +0300 Subject: [PATCH 02/47] Tools: added name to GIT_Success.txt --- Tools/GIT_Test/GIT_Success.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index a475ba17f1..a8e96d1165 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -206,3 +206,4 @@ Amr Attia Alessandro Martini Eren Mert YİĞİT Prashant Powar +Barış Kaya :) \ No newline at end of file From 5bc3b629f4dd42e3cd5aa2774b3cdd715a23ef5b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 21 Nov 2024 23:06:35 -0600 Subject: [PATCH 03/47] AP_BattMonitor: add support for ina3221 triple-channel current/voltage sensor --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 6 + libraries/AP_BattMonitor/AP_BattMonitor.h | 2 + .../AP_BattMonitor/AP_BattMonitor_INA3221.cpp | 253 ++++++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_INA3221.h | 100 +++++++ .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_config.h | 5 + 6 files changed, 367 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 10a14fd1f3..f3756ed1e5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -21,6 +21,7 @@ #include "AP_BattMonitor_EFI.h" #include "AP_BattMonitor_INA2xx.h" #include "AP_BattMonitor_INA239.h" +#include "AP_BattMonitor_INA3221.h" #include "AP_BattMonitor_LTC2946.h" #include "AP_BattMonitor_Torqeedo.h" #include "AP_BattMonitor_FuelLevel_Analog.h" @@ -604,6 +605,11 @@ AP_BattMonitor::init() drivers[instance] = NEW_NOTHROW AP_BattMonitor_Scripting(*this, state[instance], _params[instance]); break; #endif // AP_BATTERY_SCRIPTING_ENABLED +#if AP_BATTERY_INA3221_ENABLED + case Type::INA3221: + drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA3221(*this, state[instance], _params[instance]); + break; +#endif // AP_BATTERY_INA3221_ENABLED case Type::NONE: default: break; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 0b55e2846d..efdfbabef4 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -70,6 +70,7 @@ class AP_BattMonitor friend class AP_BattMonitor_INA239; friend class AP_BattMonitor_LTC2946; friend class AP_BattMonitor_AD7091R5; + friend class AP_BattMonitor_INA3221; friend class AP_BattMonitor_Torqeedo; friend class AP_BattMonitor_FuelLevel_Analog; @@ -116,6 +117,7 @@ public: EFI = 27, AD7091R5 = 28, Scripting = 29, + INA3221 = 30, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp new file mode 100644 index 0000000000..0deca24004 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp @@ -0,0 +1,253 @@ +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_INA3221_ENABLED + +#include "AP_BattMonitor_INA3221.h" + +#include + +#include +#include +#include + +#define INA3221_DEBUGGING 0 + +#if INA3221_DEBUGGING +#include +#define debug(fmt, args ...) do {printf("INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +// #define debug(fmt, args ...) do {gcs().send_text(MAV_SEVERITY_INFO, "INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else +#define debug(fmt, args ...) +#endif + +#ifndef HAL_BATTMON_INA3221_BUS +#define HAL_BATTMON_INA3221_BUS 0 +#endif + +#ifndef HAL_BATTMON_INA3221_ADDR +#define HAL_BATTMON_INA3221_ADDR 64 +#endif + +struct AP_BattMonitor_INA3221::AddressDriver AP_BattMonitor_INA3221::address_driver[HAL_BATTMON_INA3221_MAX_DEVICES]; +uint8_t AP_BattMonitor_INA3221::address_driver_count; + +const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = { + + // Param indexes must be between 56 and 61 to avoid conflict with other battery monitor param tables loaded by pointer + + // @Param: I2C_BUS + // @DisplayName: Battery monitor I2C bus number + // @Description: Battery monitor I2C bus number + // @Range: 0 3 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("I2C_BUS", 56, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS), + + // @Param: I2C_ADDR + // @DisplayName: Battery monitor I2C address + // @Description: Battery monitor I2C address. If this is zero then probe list of supported addresses + // @Range: 0 127 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("I2C_ADDR", 57, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR), + + // @Param: CHANNEL + // @DisplayName: INA3221 channel + // @Description: INA3221 channel to return data for + // @Range: 1 3 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("CHANNEL", 58, AP_BattMonitor_INA3221, channel, 1), + + AP_GROUPEND +}; + +extern const AP_HAL::HAL &hal; + +AP_BattMonitor_INA3221::AP_BattMonitor_INA3221( + AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms) : + AP_BattMonitor_Backend(mon, mon_state, params) +{ + AP_Param::setup_object_defaults(this, var_info); + _state.var_info = var_info; +} + +bool AP_BattMonitor_INA3221::AddressDriver::read_register(uint8_t addr, uint16_t &ret) +{ + if (!dev->transfer(&addr, 1, (uint8_t*)&ret, 2)) { + return false; + } + ret = be16toh(ret); + return true; +} + +bool AP_BattMonitor_INA3221::AddressDriver::write_register(uint8_t addr, uint16_t val) +{ + uint8_t buf[3] { addr, uint8_t(val >> 8), uint8_t(val & 0xFF) }; + + return dev->transfer(buf, sizeof(buf), nullptr, 0); +} + +#define REG_CONFIGURATION 0x00 +#define REG_MANUFACTURER_ID 0xFE +#define REG_DIE_ID 0xFF +void AP_BattMonitor_INA3221::init() +{ + debug("INA3221: probe @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); + + // check to see if we already have the underlying driver reading the bus: + for (uint8_t i=0; idev) { + continue; + } + if (d->address != i2c_address.get()) { + continue; + } + if (d->bus != i2c_bus.get()) { + continue; + } + debug("Reusing INA3221 driver @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); + AddressDriver::StateList *state = NEW_NOTHROW AddressDriver::StateList; + if (state == nullptr) { + return; + } + state->channel = channel.get(); + state->next = d->statelist; + state->state = &_state; + d->statelist = state; + d->channel_mask |= (1U << (uint8_t(channel.get()))); + return; + } + + if (address_driver_count == ARRAY_SIZE(address_driver)) { + debug("INA3221: out of address drivers"); + return; + } + + AddressDriver *d = &address_driver[address_driver_count]; + d->dev = std::move(hal.i2c_mgr->get_device(i2c_bus, i2c_address, 100000, true, 20)); + if (!d->dev) { + return; + } + d->bus = i2c_bus; + d->address = i2c_address; + + WITH_SEMAPHORE(d->dev->get_semaphore()); + + // check manufacturer_id + uint16_t manufacturer_id; + if (!d->read_register(REG_MANUFACTURER_ID, manufacturer_id)) { + debug("read register (%u (0x%02x)) failed", REG_MANUFACTURER_ID, REG_MANUFACTURER_ID); + return; + } + if (manufacturer_id != 0x5449) { // 8.6.1 p24 + debug("Bad manufacturer_id: 0x%02x", manufacturer_id); + return; + } + + uint16_t die_id; + if (!d->read_register(REG_DIE_ID, die_id)) { + debug("Bad die: 0x%02x", manufacturer_id); + return; + } + if (die_id != 0x3220) { // 8.6.1 p24 + return; + } + + // reset the device: + union { + struct PACKED { + uint16_t mode : 3; + uint16_t shunt_voltage_conversiontime : 3; + uint16_t bus_voltage_conversiontime : 3; + uint16_t averaging_mode : 3; + uint16_t ch1_enable : 1; + uint16_t ch2_enable : 1; + uint16_t ch3_enable : 1; + uint16_t reset : 1; + } bits; + uint16_t word; + } configuration { + 0b111, // continuous operation + 0b111, // 8ms conversion time for shunt voltage + 0b111, // 8ms conversion time for bus voltage + 0b111, // 1024 samples / average + 0b1, // enable ch1 + 0b1, // enable ch2 + 0b1, // enable ch3 + 0b0 // don't reset... + }; + + if (!d->write_register(REG_CONFIGURATION, configuration.word)) { + return; + } + + AddressDriver::StateList *state = NEW_NOTHROW AddressDriver::StateList; + if (state == nullptr) { + return; + } + state->channel = channel.get(); + state->next = d->statelist; + state->state = &_state; + d->statelist = state; + d->channel_mask |= (1U << (uint8_t(channel.get()))); + + debug("Found INA3221 @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); + + address_driver_count++; + + d->register_timer(); +} + +void AP_BattMonitor_INA3221::AddressDriver::register_timer(void) +{ + dev->register_periodic_callback( + 100000, + FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA3221::AddressDriver::timer, void)); +} + +void AP_BattMonitor_INA3221::AddressDriver::timer(void) +{ + for (uint8_t i=1; i<=3; i++) { + if ((channel_mask & (1U<next) { + if (state->channel != i) { + continue; + } + WITH_SEMAPHORE(state->sem); + state->state->voltage = bus_voltage/32768.0 * 26; + state->state->current_amps = shunt_voltage * 0.56f; + state->state->last_time_micros = AP_HAL::micros(); + } + } +} + +void AP_BattMonitor_INA3221::read() +{ + static uint32_t last_print; + if (AP_HAL::millis() - last_print > 5000) { + last_print = AP_HAL::millis(); + gcs().send_text(MAV_SEVERITY_INFO, "%u: voltage:%f current:%f", (unsigned)_state.last_time_micros, _state.voltage, _state.current_amps); + } +} + +#endif // AP_BATTERY_INA3221_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h new file mode 100644 index 0000000000..d551800960 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h @@ -0,0 +1,100 @@ +#pragma once + +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_INA3221_ENABLED + +/* + * + * Datasheet: https://www.ti.com/lit/ds/symlink/ina3221.pdf?ts=1597369254046 + */ + +// The INA3221 takes two measurements for each channel: one for shunt voltage +// and one for bus voltage. Each measurement can be independently or +// sequentially measured, based on the mode setting (bits 2-0 in the +// Configuration register). When the INA3221 is in normal operating mode +// (that is, the MODE bits of the Configuration register are set to 111), the +// device continuously converts a shunt-voltage reading followed by a +// bus-voltage reading. This procedure converts one channel, and then continues +// to the shunt voltage reading of the next enabled channel, followed by the +// bus-voltage reading for that channel, and so on, until all enabled channels +// have been measured. The programmed Configuration register mode setting +// applies to all channels. Any channels that are not enabled are bypassed in +// the measurement sequence, regardless of mode setting. + + +// 8.3.3 Software Reset +// The INA3221 features a software reset that reinitializes the device and +// register settings to default power-up values without having to cycle power +// to the device. Use bit 15 (RST) of the Configuration register to perform a +// software reset. Setting RST reinitializes all registers and settings to the +// default power state with the exception of the power-valid output state. If a +// software reset is issued, the INA3221 holds the output of the PV pin until +// the power-valid detection sequence completes. The Power-Valid UpperLimit and +// Power-Valid Lowerlimit registers return to the default state when the +// software reset has been issued. Therefore, any reprogrammed limit registers +// are reset, resulting in the original power-valid thresholds validating the +// power-valid conditions. This architecture prevents interruption to circuitry +// connected to the powervalid output during a software reset event. + +// The INA3221 has programmable conversion times for both the shunt- and +// bus-voltage measurements. The selectable conversion times for these +// measurements range from 140μs to 8.244ms. + + +#include "AP_BattMonitor.h" +#include "AP_BattMonitor_Backend.h" + +#ifndef HAL_BATTMON_INA3221_MAX_DEVICES +#define HAL_BATTMON_INA3221_MAX_DEVICES 1 +#endif + +class AP_BattMonitor_INA3221 : public AP_BattMonitor_Backend +{ +public: + /// Constructor + AP_BattMonitor_INA3221(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms); + + void init() override; + + /// Read the battery voltage and current. Should be called at 10hz + void read() override; + + bool has_current() const override { + return true; + } + + static const struct AP_Param::GroupInfo var_info[]; + +private: + + AP_Int8 i2c_bus; + AP_Int8 i2c_address; + AP_Int8 channel; + + static struct AddressDriver { + bool read_register(uint8_t addr, uint16_t &ret); + bool write_register(uint8_t addr, uint16_t val); + void timer(void); + void register_timer(); + + AP_HAL::OwnPtr dev; + uint8_t bus; + uint8_t address; + uint8_t channel_mask; + + struct StateList { + struct StateList *next; + uint8_t channel; + AP_BattMonitor::BattMonitor_State *state; + HAL_Semaphore sem; + }; + StateList *statelist; + + } address_driver[HAL_BATTMON_INA3221_MAX_DEVICES]; + static uint8_t address_driver_count; +}; + +#endif // AP_BATTERY_INA3221_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index aceee7c05b..eac0d2b02b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -21,7 +21,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: MONITOR // @DisplayName: Battery monitoring // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting,30:INA3221 // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index 3159b075d6..6f73850900 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_config.h @@ -70,6 +70,11 @@ #define AP_BATTERY_INA2XX_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && (BOARD_FLASH_SIZE > 1024)) #endif +#ifndef AP_BATTERY_INA3221_ENABLED +// turned on in hwdefs (except for sim test), requires config +#define AP_BATTERY_INA3221_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_BATTERY_LTC2946_ENABLED #define AP_BATTERY_LTC2946_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && defined(HAL_BATTMON_LTC2946_BUS) && defined(HAL_BATTMON_LTC2946_ADDR)) #endif From 4cfbeb11e328d89d6dee6d7a96d0318a5d7b970b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 21 Nov 2024 23:06:35 -0600 Subject: [PATCH 04/47] SITL: add support for ina3221 triple-channel current/voltage sensor --- libraries/SITL/SIM_I2C.cpp | 7 ++ libraries/SITL/SIM_INA3221.cpp | 143 +++++++++++++++++++++++++++++++++ libraries/SITL/SIM_INA3221.h | 89 ++++++++++++++++++++ libraries/SITL/SIM_config.h | 3 + 4 files changed, 242 insertions(+) create mode 100644 libraries/SITL/SIM_INA3221.cpp create mode 100644 libraries/SITL/SIM_INA3221.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 196301942d..8d1b1daf0a 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -37,6 +37,7 @@ #include "SIM_MS5525.h" #include "SIM_MS5611.h" #include "SIM_QMC5883L.h" +#include "SIM_INA3221.h" #include @@ -85,6 +86,9 @@ static IS31FL3195 is31fl3195; #if AP_SIM_COMPASS_QMC5883L_ENABLED static QMC5883L qmc5883l; #endif +#if AP_SIM_INA3221_ENABLED +static INA3221 ina3221; +#endif struct i2c_device_at_address { uint8_t bus; @@ -102,6 +106,9 @@ struct i2c_device_at_address { { 1, 0x39, ignored }, // NCP5623C { 1, 0x40, ignored }, // KellerLD { 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4 +#if AP_SIM_INA3221_ENABLED + { 1, 0x42, ina3221 }, +#endif { 1, 0x77, tsys01 }, { 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13 { 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13 diff --git a/libraries/SITL/SIM_INA3221.cpp b/libraries/SITL/SIM_INA3221.cpp new file mode 100644 index 0000000000..3a12d36342 --- /dev/null +++ b/libraries/SITL/SIM_INA3221.cpp @@ -0,0 +1,143 @@ +#include "SIM_config.h" + +#if AP_SIM_INA3221_ENABLED + +#include "SIM_INA3221.h" + +#include + +SITL::INA3221::INA3221() +{ + writable_registers.set(0); + writable_registers.set(7); + writable_registers.set(8); + writable_registers.set(9); + writable_registers.set(10); + writable_registers.set(11); + writable_registers.set(12); + writable_registers.set(14); + writable_registers.set(15); + writable_registers.set(16); + writable_registers.set(254); + writable_registers.set(255); + + reset(); +} + +void SITL::INA3221::reset() +{ + // from page 24 of datasheet: + registers.byname.configuration.word = 0x7127; + registers.byname.Channel_1_Shunt_Voltage = 0x0; + registers.byname.Channel_1_Bus_Voltage = 0x0; + registers.byname.Channel_2_Shunt_Voltage = 0x0; + registers.byname.Channel_2_Bus_Voltage = 0x0; + registers.byname.Channel_3_Shunt_Voltage = 0x0; + registers.byname.Channel_3_Bus_Voltage = 0x0; + registers.byname.Channel_1_CriticalAlertLimit = 0x7FF8; + registers.byname.Channel_1_WarningAlertLimit = 0x7FF8; + registers.byname.Channel_2_CriticalAlertLimit = 0x7FF8; + registers.byname.Channel_2_WarningAlertLimit = 0x7FF8; + registers.byname.Channel_3_CriticalAlertLimit = 0x7FF8; + registers.byname.Channel_3_WarningAlertLimit = 0x7FF8; + registers.byname.Shunt_VoltageSum = 0x0; + registers.byname.Shunt_VoltageSumLimit = 0x7FFE; + registers.byname.Mask_Enable = 0x0002; + registers.byname.Power_ValidUpperLimit = 0x2710; + registers.byname.Power_ValidLowerLimit = 0x2328; + registers.byname.ManufacturerID = MANUFACTURER_ID; + registers.byname.Die_ID = DIE_ID; +} + +int SITL::INA3221::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + if (data->nmsgs == 2) { + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + if (data->msgs[1].flags != I2C_M_RD) { + AP_HAL::panic("Unexpected flags"); + } + const uint8_t reg_addr = data->msgs[0].buf[0]; + const uint16_t register_value = registers.word[reg_addr]; + data->msgs[1].buf[0] = register_value >> 8; + data->msgs[1].buf[1] = register_value & 0xFF; + data->msgs[1].len = 2; + return 0; + } + + if (data->nmsgs == 1) { + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + const uint8_t reg_addr = data->msgs[0].buf[0]; + if (!writable_registers.get(reg_addr)) { + AP_HAL::panic("Register 0x%02x is not writable!", reg_addr); + } + const uint16_t register_value = data->msgs[0].buf[2] << 8 | data->msgs[0].buf[1]; + registers.word[reg_addr] = register_value; + return 0; + } + + return -1; +}; + +static uint16_t convert_voltage(float voltage) { + return (voltage / 26) * 32768; +} + + +void SITL::INA3221::update(const class Aircraft &aircraft) +{ + if (registers.byname.configuration.bits.reset != 0) { + reset(); + } + + // update readings + if (registers.byname.configuration.bits.mode == 0b000 || + registers.byname.configuration.bits.mode == 0b100) { + // power-off + return; + } + + const bool update_shunt = registers.byname.configuration.bits.mode & 0b001; + const bool update_bus = registers.byname.configuration.bits.mode & 0b010; + if ((registers.byname.configuration.bits.mode & 0b100) == 0) { + // single-shot only + registers.byname.configuration.bits.mode &= ~0b011; + } + + // channel 2 gets the first simulated battery's voltage and current: + // see 8.6.6.2 on page 27 for the whole "40uV" thing + if (registers.byname.configuration.bits.ch1_enable) { + if (update_bus) { + float fakevoltage = 12.3; + registers.byname.Channel_1_Bus_Voltage = fakevoltage; // FIXME + } + if (update_shunt) { + float fakecurrent = 7.6; + registers.byname.Channel_1_Shunt_Voltage = fakecurrent/0.56; // FIXME + } + } + + if (registers.byname.configuration.bits.ch2_enable) { + if (update_shunt) { + registers.byname.Channel_2_Shunt_Voltage = AP::sitl()->state.battery_current/26 * 32768; // FIXME + } + if (update_bus) { + registers.byname.Channel_2_Bus_Voltage = AP::sitl()->state.battery_voltage/26 * 32768; // FIXME + } + } + + if (registers.byname.configuration.bits.ch3_enable) { + if (update_bus) { + registers.byname.Channel_3_Bus_Voltage = convert_voltage(3.1415); + } + if (update_shunt) { + registers.byname.Channel_3_Shunt_Voltage = 2.718/0.56; + } + } + +} + +#endif // AP_SIM_INA3221_ENABLED diff --git a/libraries/SITL/SIM_INA3221.h b/libraries/SITL/SIM_INA3221.h new file mode 100644 index 0000000000..93924c2421 --- /dev/null +++ b/libraries/SITL/SIM_INA3221.h @@ -0,0 +1,89 @@ +#include "SIM_config.h" + +#if AP_SIM_INA3221_ENABLED + +#include "SIM_I2CDevice.h" + +#include + +/* + +Testing: + +param set BATT_MONITOR 30 +reboot +param set BATT_I2C_ADDR 0x42 +param set BATT_I2C_BUS 1 +param set BATT_CHANNEL 2 +reboot + +*/ + +namespace SITL { + +class INA3221 : public I2CDevice +{ +public: + + INA3221(); + + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + void update(const class Aircraft &aircraft) override; + +private: + + static const uint16_t MANUFACTURER_ID = 0b0101010001001001; // from datasheet p25 + static const uint16_t DIE_ID = 0b0011001000100000; // from datasheet p25 + + // All 16-bit INA3221 registers are two 8-bit bytes via the I2C + // interface. Table 4 shows a register map for the INA3221. + union Registers { + uint16_t word[256]; + struct PACKED ByName { + union { + uint16_t word; + struct PACKED { + uint16_t mode : 3; + uint16_t shunt_voltage_conversiontime : 3; + uint16_t bus_voltage_conversiontime : 3; + uint16_t averaging_mode : 3; + uint16_t ch1_enable : 1; + uint16_t ch2_enable : 1; + uint16_t ch3_enable : 1; + uint16_t reset : 1; + } bits; + } configuration; + uint16_t Channel_1_Shunt_Voltage; + uint16_t Channel_1_Bus_Voltage; + uint16_t Channel_2_Shunt_Voltage; + uint16_t Channel_2_Bus_Voltage; + uint16_t Channel_3_Shunt_Voltage; + uint16_t Channel_3_Bus_Voltage; + uint16_t Channel_1_CriticalAlertLimit; + uint16_t Channel_1_WarningAlertLimit; + uint16_t Channel_2_CriticalAlertLimit; + uint16_t Channel_2_WarningAlertLimit; + uint16_t Channel_3_CriticalAlertLimit; + uint16_t Channel_3_WarningAlertLimit; + uint16_t Shunt_VoltageSum; + uint16_t Shunt_VoltageSumLimit; + uint16_t Mask_Enable; + uint16_t Power_ValidUpperLimit; + uint16_t Power_ValidLowerLimit; + uint16_t unused[236]; + uint16_t ManufacturerID; + uint16_t Die_ID; + } byname; + } registers; + + // 256 2-byte registers: + assert_storage_size assert_storage_size_registers_reg; + + Bitmask<256> writable_registers; + + void reset(); +}; + +} // namespace SITL + +#endif // AP_SIM_INA3221_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 557c3077ed..d417577249 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -144,3 +144,6 @@ #define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED) #endif +#ifndef AP_SIM_INA3221_ENABLED +#define AP_SIM_INA3221_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif From cc4110140cfaa34556035cea3d9b3663858d3699 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 21 Nov 2024 23:06:35 -0600 Subject: [PATCH 05/47] Tools: add support for ina3221 triple-channel current/voltage sensor --- Tools/autotest/ardusub.py | 46 ++++++++++++++++++++++++++++++++++ Tools/scripts/build_options.py | 1 + 2 files changed, 47 insertions(+) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 309c22b8c4..a0eb71b5da 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -947,6 +947,51 @@ class AutoTestSub(vehicle_test_suite.TestSuite): self.wait_ready_to_arm() self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG) + def INA3221(self): + '''test INA3221 driver''' + self.set_parameters({ + "BATT2_MONITOR": 30, + "BATT3_MONITOR": 30, + "BATT4_MONITOR": 30, + }) + self.reboot_sitl() + self.set_parameters({ + "BATT2_I2C_ADDR": 0x42, + "BATT2_I2C_BUS": 1, + "BATT2_CHANNEL": 1, + + "BATT3_I2C_ADDR": 0x42, + "BATT3_I2C_BUS": 1, + "BATT3_CHANNEL": 2, + + "BATT4_I2C_ADDR": 0x42, + "BATT4_I2C_BUS": 1, + "BATT4_CHANNEL": 3, + }) + self.reboot_sitl() + + seen_1 = False + seen_3 = False + tstart = self.get_sim_time() + while not (seen_1 and seen_3): + m = self.assert_receive_message('BATTERY_STATUS') + print(self.dump_message_verbose(m)) + if self.get_sim_time() - tstart > 1: + break + continue + if m.id == 1: + self.assert_message_field_values(m, { + "current_battery": 7.28 * 100, + }) + # "voltages[0]": 12 * 1000, + seen_1 = True + if m.id == 3: + self.assert_message_field_values(m, { + "current_battery": 2.24 * 100, + }) + # "voltages[0]": 3.14159 * 1000, + seen_3 = True + def tests(self): '''return list of all tests''' ret = super(AutoTestSub, self).tests() @@ -978,6 +1023,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite): self.SetGlobalOrigin, self.BackupOrigin, self.FuseMag, + self.INA3221, ]) return ret diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 1c259adeae..b140fe125b 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -56,6 +56,7 @@ BUILD_OPTIONS = [ Feature('Battery', 'BATTERY_FUELLEVEL_ANALOG', 'AP_BATTERY_FUELLEVEL_ANALOG_ENABLED', 'Enable Analog Fuel level battry monitor', 0, None), # noqa: E501 Feature('Battery', 'BATTERY_SMBUS', 'AP_BATTERY_SMBUS_ENABLED', 'Enable SMBUS battery monitor', 0, None), Feature('Battery', 'BATTERY_INA2XX', 'AP_BATTERY_INA2XX_ENABLED', 'Enable INA2XX battery monitor', 0, None), + Feature('Battery', 'BATTERY_INA3221', 'AP_BATTERY_INA3221_ENABLED', 'Enable INA3221 battery monitor', 0, None), Feature('Battery', 'BATTERY_SYNTHETIC_CURRENT', 'AP_BATTERY_SYNTHETIC_CURRENT_ENABLED', 'Enable Synthetic Current monitor', 0, None), # noqa: E501 Feature('Battery', 'BATTERY_ESC_TELEM_OUT', 'AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED', 'Enable Ability to put battery monitor data into ESC telem stream', 0, None), # noqa: E501 Feature('Battery', 'BATTERY_SUM', 'AP_BATTERY_SUM_ENABLED', 'Enable Synthetic sum-of-other-batteries backend', 0, None), # noqa: E501 From 190c3aa7abd9c24e1b66c8072e4f328e3b991408 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 23 Nov 2024 19:52:24 -0600 Subject: [PATCH 06/47] AP_BattMonitor: INA3221: fix up based on real experience * correctly validate channel parameter and improve other parameter access * dynamically enable channels to avoid spending time converting unused channels * implement tracking of reading health * correct reading scaling by using datasheet values * accumulate measured current to track used mAh and Wh * make configurable using #defines (and hwdef) for integrators * correctly separate and lock frontend and backend state. Note that _state of frontend can only be accessed in `read()` method. --- .../AP_BattMonitor/AP_BattMonitor_INA3221.cpp | 213 +++++++++++++----- .../AP_BattMonitor/AP_BattMonitor_INA3221.h | 14 +- 2 files changed, 167 insertions(+), 60 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp index 0deca24004..cad3a04acc 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp @@ -28,6 +28,40 @@ #define HAL_BATTMON_INA3221_ADDR 64 #endif +#ifndef HAL_BATTMON_INA3221_SHUNT_OHMS +#define HAL_BATTMON_INA3221_SHUNT_OHMS 0.001 +#endif + +#define HAL_BATTMON_INA3221_CONV_TIME_140US 0b000 +#define HAL_BATTMON_INA3221_CONV_TIME_204US 0b001 +#define HAL_BATTMON_INA3221_CONV_TIME_332US 0b010 +#define HAL_BATTMON_INA3221_CONV_TIME_588US 0b011 +#define HAL_BATTMON_INA3221_CONV_TIME_1100US 0b100 +#define HAL_BATTMON_INA3221_CONV_TIME_2116US 0b101 +#define HAL_BATTMON_INA3221_CONV_TIME_4156US 0b110 +#define HAL_BATTMON_INA3221_CONV_TIME_8244US 0b111 + +#define HAL_BATTMON_INA3221_AVG_MODE_1 0b000 +#define HAL_BATTMON_INA3221_AVG_MODE_4 0b001 +#define HAL_BATTMON_INA3221_AVG_MODE_16 0b010 +#define HAL_BATTMON_INA3221_AVG_MODE_64 0b011 +#define HAL_BATTMON_INA3221_AVG_MODE_128 0b100 +#define HAL_BATTMON_INA3221_AVG_MODE_256 0b101 +#define HAL_BATTMON_INA3221_AVG_MODE_512 0b110 +#define HAL_BATTMON_INA3221_AVG_MODE_1024 0b111 + +#ifndef HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL +#define HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US +#endif + +#ifndef HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL +#define HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US +#endif + +#ifndef HAL_BATTMON_INA3221_AVG_MODE_SEL +#define HAL_BATTMON_INA3221_AVG_MODE_SEL HAL_BATTMON_INA3221_AVG_MODE_1024 +#endif + struct AP_BattMonitor_INA3221::AddressDriver AP_BattMonitor_INA3221::address_driver[HAL_BATTMON_INA3221_MAX_DEVICES]; uint8_t AP_BattMonitor_INA3221::address_driver_count; @@ -93,9 +127,55 @@ bool AP_BattMonitor_INA3221::AddressDriver::write_register(uint8_t addr, uint16_ #define REG_CONFIGURATION 0x00 #define REG_MANUFACTURER_ID 0xFE #define REG_DIE_ID 0xFF + +bool AP_BattMonitor_INA3221::AddressDriver::write_config(void) +{ + // update device configuration + union { + struct PACKED { + uint16_t mode : 3; + uint16_t shunt_voltage_conversiontime : 3; + uint16_t bus_voltage_conversiontime : 3; + uint16_t averaging_mode : 3; + uint16_t ch1_enable : 1; + uint16_t ch2_enable : 1; + uint16_t ch3_enable : 1; + uint16_t reset : 1; + } bits; + uint16_t word; + } configuration {{ + 0b111, // continuous operation + HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL, + HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL, + HAL_BATTMON_INA3221_AVG_MODE_SEL, + // dynamically enable channels to not waste time converting unused data + (channel_mask & (1 << 1)) != 0, // enable ch1? + (channel_mask & (1 << 2)) != 0, // enable ch2? + (channel_mask & (1 << 3)) != 0, // enable ch3? + 0b0, // don't reset... + }}; + + if (!write_register(REG_CONFIGURATION, configuration.word)) { + return false; + } + + dev_channel_mask = channel_mask; // what's actually in the device now + + return true; +} + void AP_BattMonitor_INA3221::init() { - debug("INA3221: probe @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); + uint8_t dev_address = i2c_address.get(); + uint8_t dev_bus = i2c_bus.get(); + uint8_t dev_channel = channel.get(); + + if ((dev_channel < 1) || (dev_channel > 3)) { + debug("INA3221: nonexistent channel"); + return; + } + + debug("INA3221: probe ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus); // check to see if we already have the underlying driver reading the bus: for (uint8_t i=0; idev) { continue; } - if (d->address != i2c_address.get()) { + if (d->address != dev_address) { continue; } - if (d->bus != i2c_bus.get()) { + if (d->bus != dev_bus) { continue; } - debug("Reusing INA3221 driver @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); - AddressDriver::StateList *state = NEW_NOTHROW AddressDriver::StateList; - if (state == nullptr) { + debug("Reusing INA3221 driver @0x%02x on bus %u", dev_address, dev_bus); + address_driver_state = NEW_NOTHROW AddressDriver::StateList; + if (address_driver_state == nullptr) { return; } - state->channel = channel.get(); - state->next = d->statelist; - state->state = &_state; - d->statelist = state; - d->channel_mask |= (1U << (uint8_t(channel.get()))); + address_driver_state->channel = dev_channel; + address_driver_state->next = d->statelist; + d->statelist = address_driver_state; + d->channel_mask |= (1 << dev_channel); return; } @@ -157,45 +236,20 @@ void AP_BattMonitor_INA3221::init() return; } - // reset the device: - union { - struct PACKED { - uint16_t mode : 3; - uint16_t shunt_voltage_conversiontime : 3; - uint16_t bus_voltage_conversiontime : 3; - uint16_t averaging_mode : 3; - uint16_t ch1_enable : 1; - uint16_t ch2_enable : 1; - uint16_t ch3_enable : 1; - uint16_t reset : 1; - } bits; - uint16_t word; - } configuration { - 0b111, // continuous operation - 0b111, // 8ms conversion time for shunt voltage - 0b111, // 8ms conversion time for bus voltage - 0b111, // 1024 samples / average - 0b1, // enable ch1 - 0b1, // enable ch2 - 0b1, // enable ch3 - 0b0 // don't reset... - }; - - if (!d->write_register(REG_CONFIGURATION, configuration.word)) { + d->channel_mask = (1 << dev_channel); + if (!d->write_config()) { return; } - AddressDriver::StateList *state = NEW_NOTHROW AddressDriver::StateList; - if (state == nullptr) { + address_driver_state = NEW_NOTHROW AddressDriver::StateList; + if (address_driver_state == nullptr) { return; } - state->channel = channel.get(); - state->next = d->statelist; - state->state = &_state; - d->statelist = state; - d->channel_mask |= (1U << (uint8_t(channel.get()))); + address_driver_state->channel = dev_channel; + address_driver_state->next = d->statelist; + d->statelist = address_driver_state; - debug("Found INA3221 @0x%02x on bus %u", i2c_address.get(), i2c_bus.get()); + debug("Found INA3221 ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus); address_driver_count++; @@ -211,6 +265,16 @@ void AP_BattMonitor_INA3221::AddressDriver::register_timer(void) void AP_BattMonitor_INA3221::AddressDriver::timer(void) { + bool healthy = true; + + if (channel_mask != dev_channel_mask) { + if (write_config()) { // update enabled channels + return; // data is now out of date, read it next time + } + // continue on to reading if update failed so health gets cleared + healthy = false; + } + for (uint8_t i=1; i<=3; i++) { if ((channel_mask & (1U<> 3)*8e-3; + // 2s complement number with 3 lowest bits not used, 1 count is 40uV + const float shunt_voltage = ((int16_t)shunt_val >> 3)*40e-6; + const float shunt_resistance = HAL_BATTMON_INA3221_SHUNT_OHMS; + const float shunt_current = shunt_voltage/shunt_resistance; // I = V/R + + // transfer readings to state for (auto *state = statelist; state != nullptr; state = state->next) { if (state->channel != i) { continue; } WITH_SEMAPHORE(state->sem); - state->state->voltage = bus_voltage/32768.0 * 26; - state->state->current_amps = shunt_voltage * 0.56f; - state->state->last_time_micros = AP_HAL::micros(); + + // calculate time since last data read + const uint32_t tnow = AP_HAL::micros(); + const uint32_t dt_us = tnow - state->last_time_micros; + + state->healthy = healthy; + state->voltage = bus_voltage; + state->current_amps = shunt_current; + + // update current drawn since last reading for read to accumulate + if (state->last_time_micros != 0 && dt_us < 2000000) { + const float mah = calculate_mah(state->current_amps, dt_us); + state->delta_mah += mah; + state->delta_wh += 0.001 * mah * state->voltage; + } + + state->last_time_micros = tnow; } } } void AP_BattMonitor_INA3221::read() { - static uint32_t last_print; - if (AP_HAL::millis() - last_print > 5000) { - last_print = AP_HAL::millis(); - gcs().send_text(MAV_SEVERITY_INFO, "%u: voltage:%f current:%f", (unsigned)_state.last_time_micros, _state.voltage, _state.current_amps); + if (address_driver_state == nullptr) { + return; } + + WITH_SEMAPHORE(address_driver_state->sem); + // copy state data to front-end under semaphore + _state.healthy = address_driver_state->healthy; + _state.voltage = address_driver_state->voltage; + _state.current_amps = address_driver_state->current_amps; + _state.consumed_mah += address_driver_state->delta_mah; + _state.consumed_wh += address_driver_state->delta_wh; + _state.last_time_micros = address_driver_state->last_time_micros; + + address_driver_state->delta_mah = 0; + address_driver_state->delta_wh = 0; } #endif // AP_BATTERY_INA3221_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h index d551800960..898753207c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h @@ -77,6 +77,7 @@ private: static struct AddressDriver { bool read_register(uint8_t addr, uint16_t &ret); bool write_register(uint8_t addr, uint16_t val); + bool write_config(void); void timer(void); void register_timer(); @@ -84,17 +85,26 @@ private: uint8_t bus; uint8_t address; uint8_t channel_mask; + uint8_t dev_channel_mask; struct StateList { struct StateList *next; - uint8_t channel; - AP_BattMonitor::BattMonitor_State *state; HAL_Semaphore sem; + uint8_t channel; + + bool healthy; + float voltage; + float current_amps; + float delta_mah; + float delta_wh; + uint32_t last_time_micros; }; StateList *statelist; } address_driver[HAL_BATTMON_INA3221_MAX_DEVICES]; static uint8_t address_driver_count; + + AddressDriver::StateList *address_driver_state; }; #endif // AP_BATTERY_INA3221_ENABLED From 6abbefde12bd9a53eefd5d73276aa99f3a47ca5d Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Fri, 29 Nov 2024 16:45:38 -0600 Subject: [PATCH 07/47] autotest: allow assertion of mavlink message array elements --- Tools/autotest/vehicle_test_suite.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 3c08f09396..a5535d73da 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -4364,7 +4364,13 @@ class TestSuite(ABC): def message_has_field_values(self, m, fieldvalues, verbose=True, epsilon=None): for (fieldname, value) in fieldvalues.items(): - got = getattr(m, fieldname) + if "[" in fieldname: # fieldname == "arrayname[index]" + assert fieldname[-1] == "]", fieldname + arrayname, index = fieldname.split("[", 1) + index = int(index[:-1]) + got = getattr(m, arrayname)[index] + else: + got = getattr(m, fieldname) value_string = value got_string = got From adfc415cffc2d04b8b3618ba2301ba5904454f87 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 1 Dec 2024 12:30:04 -0600 Subject: [PATCH 08/47] SITL: fix testing and simulated register scaling * make test actually test something * fix scaling to match datasheet values --- libraries/SITL/SIM_INA3221.cpp | 54 ++++++++++++++++++++++++---------- 1 file changed, 39 insertions(+), 15 deletions(-) diff --git a/libraries/SITL/SIM_INA3221.cpp b/libraries/SITL/SIM_INA3221.cpp index 3a12d36342..61c5308b22 100644 --- a/libraries/SITL/SIM_INA3221.cpp +++ b/libraries/SITL/SIM_INA3221.cpp @@ -82,8 +82,34 @@ int SITL::INA3221::rdwr(I2C::i2c_rdwr_ioctl_data *&data) return -1; }; -static uint16_t convert_voltage(float voltage) { - return (voltage / 26) * 32768; +static uint16_t convert_voltage(float voltage_v) { + // 8mV per count, register value is x8 (3 lowest bits not used) + float volt_counts = (voltage_v/8e-3)*8; + if (volt_counts < INT16_MIN) { + volt_counts = INT16_MIN; + } else if (volt_counts > INT16_MAX) { + volt_counts = INT16_MAX; + } + + // register value is signed + return (uint16_t)((int16_t)volt_counts); +} + +static uint16_t convert_current(float current_a) { + // V = IR + const float shunt_resistance_ohms = 0.001; // default value in driver + const float shunt_voltage = current_a*shunt_resistance_ohms; + + // 40uV per count, register value is x8 (3 lowest bits not used) + float shunt_counts = (shunt_voltage/40e-6)*8; + if (shunt_counts < INT16_MIN) { + shunt_counts = INT16_MIN; + } else if (shunt_counts > INT16_MAX) { + shunt_counts = INT16_MAX; + } + + // register value is signed + return (uint16_t)((int16_t)shunt_counts); } @@ -107,37 +133,35 @@ void SITL::INA3221::update(const class Aircraft &aircraft) registers.byname.configuration.bits.mode &= ~0b011; } - // channel 2 gets the first simulated battery's voltage and current: - // see 8.6.6.2 on page 27 for the whole "40uV" thing + // channel 2 gets the first simulated battery's voltage and current, others are test values: if (registers.byname.configuration.bits.ch1_enable) { + // values close to chip limits (assuming 1mOhm current shunt) if (update_bus) { - float fakevoltage = 12.3; - registers.byname.Channel_1_Bus_Voltage = fakevoltage; // FIXME + registers.byname.Channel_1_Bus_Voltage = convert_voltage(25); // max of 26V } if (update_shunt) { - float fakecurrent = 7.6; - registers.byname.Channel_1_Shunt_Voltage = fakecurrent/0.56; // FIXME + registers.byname.Channel_1_Shunt_Voltage = convert_current(160); // max of 163.8A } } if (registers.byname.configuration.bits.ch2_enable) { - if (update_shunt) { - registers.byname.Channel_2_Shunt_Voltage = AP::sitl()->state.battery_current/26 * 32768; // FIXME - } if (update_bus) { - registers.byname.Channel_2_Bus_Voltage = AP::sitl()->state.battery_voltage/26 * 32768; // FIXME + registers.byname.Channel_2_Bus_Voltage = convert_voltage(AP::sitl()->state.battery_voltage); + } + if (update_shunt) { + registers.byname.Channel_2_Shunt_Voltage = convert_current(AP::sitl()->state.battery_current); } } if (registers.byname.configuration.bits.ch3_enable) { + // values different from above to test channel switching if (update_bus) { - registers.byname.Channel_3_Bus_Voltage = convert_voltage(3.1415); + registers.byname.Channel_3_Bus_Voltage = convert_voltage(3.14159); } if (update_shunt) { - registers.byname.Channel_3_Shunt_Voltage = 2.718/0.56; + registers.byname.Channel_3_Shunt_Voltage = convert_current(2.71828); } } - } #endif // AP_SIM_INA3221_ENABLED From da4fee5a9a94511ce778a3b740990f8bcf0e12fb Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 1 Dec 2024 12:30:04 -0600 Subject: [PATCH 09/47] autotest: fix testing and simulated register scaling * make test actually test something * fix scaling to match datasheet values --- Tools/autotest/ardusub.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index a0eb71b5da..5947640790 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -956,7 +956,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite): }) self.reboot_sitl() self.set_parameters({ - "BATT2_I2C_ADDR": 0x42, + "BATT2_I2C_ADDR": 0x42, # address defined in libraries/SITL/SIM_I2C.cpp "BATT2_I2C_BUS": 1, "BATT2_CHANNEL": 1, @@ -975,21 +975,24 @@ class AutoTestSub(vehicle_test_suite.TestSuite): tstart = self.get_sim_time() while not (seen_1 and seen_3): m = self.assert_receive_message('BATTERY_STATUS') - print(self.dump_message_verbose(m)) - if self.get_sim_time() - tstart > 1: - break - continue + if self.get_sim_time() - tstart > 10: + # expected to take under 1 simulated second, but don't hang if + # e.g. the driver gets stuck + raise NotAchievedException("INA3221 status timeout") if m.id == 1: self.assert_message_field_values(m, { - "current_battery": 7.28 * 100, - }) - # "voltages[0]": 12 * 1000, + # values close to chip limits + "voltages[0]": int(25 * 1000), # millivolts + "current_battery": int(160 * 100), # centi-amps + }, epsilon=10) # allow rounding seen_1 = True + # id 2 is the first simulated battery current/voltage if m.id == 3: self.assert_message_field_values(m, { - "current_battery": 2.24 * 100, - }) - # "voltages[0]": 3.14159 * 1000, + # values different from above to test channel switching + "voltages[0]": int(3.14159 * 1000), # millivolts + "current_battery": int(2.71828 * 100), # centi-amps + }, epsilon=10) # allow rounding seen_3 = True def tests(self): From 5503ac204efd4fc97307773aef98a898046f2bb0 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 22 Nov 2024 09:52:44 -0800 Subject: [PATCH 10/47] Tools: update WSL2 use in uploader.py --- Tools/ardupilotwaf/chibios.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 785bce15bc..6c998c14f2 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -103,7 +103,7 @@ class upload_fw(Task.Task): except subprocess.CalledProcessError: #if where.exe can't find the file it returns a non-zero result which throws this exception where_python = "" - if not where_python or "\Python\Python" not in where_python or "python.exe" not in where_python: + if "python.exe" not in where_python: print(self.get_full_wsl2_error_msg("Windows python.exe not found")) return False return True From 2863dcfb94ee2ef793205597a28cecec8b72e5fe Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:10:50 +0000 Subject: [PATCH 11/47] AP_Logger: add new unit `t` for torque in N.m --- libraries/AP_Logger/LogStructure.h | 1 + libraries/AP_Logger/README.md | 1 + 2 files changed, 2 insertions(+) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index d2c014eb33..3b7c599bcd 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -67,6 +67,7 @@ const struct UnitStructure log_Units[] = { { '%', "%" }, // percent { 'S', "satellites" }, // number of satellites { 's', "s" }, // seconds + { 't', "N.m" }, // Newton meters, torque { 'q', "rpm" }, // rounds per minute. Not SI, but sometimes more intuitive than Hertz { 'r', "rad" }, // radians { 'U', "deglongitude" }, // degrees of longitude diff --git a/libraries/AP_Logger/README.md b/libraries/AP_Logger/README.md index 52b99ff583..c75ead7a94 100644 --- a/libraries/AP_Logger/README.md +++ b/libraries/AP_Logger/README.md @@ -68,6 +68,7 @@ Please keep the names consistent with Tools/autotest/param_metadata/param.py:33 | 's' | "s" | seconds| | 'q' | "rpm" | revolutions per minute| Not an SI unit, but sometimes more intuitive than Hertz| | 'r' | "rad" | radians| +| 't' | "N.m" | Newton meters | torque | | 'U' | "deglongitude" | degrees of longitude| | 'u' | "ppm" | pulses per minute| | 'v' | "V" | Volt| From d5c29735d6d52f386302499391a936d0a40f47bf Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:11:48 +0000 Subject: [PATCH 12/47] AP_DroneCAN: send incomming servo telem data to new `AP_Servo_Telem` lib --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 106 ++++++++++++++++---------- libraries/AP_DroneCAN/AP_DroneCAN.h | 17 ++++- 2 files changed, 78 insertions(+), 45 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index a8c9a531c3..8f07785773 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #if AP_DRONECAN_SERIAL_ENABLED #include "AP_DroneCAN_serial.h" @@ -1379,62 +1380,83 @@ void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const /* handle actuator status message */ +#if AP_SERVO_TELEM_ENABLED void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg) { -#if HAL_LOGGING_ENABLED - // log as CSRV message - AP::logger().Write_ServoStatus(AP_HAL::micros64(), - msg.actuator_id, - msg.position, - msg.force, - msg.speed, - msg.power_rating_pct, - 0, 0, 0, 0, 0, 0); -#endif -} + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem == nullptr) { + return; + } -#if AP_DRONECAN_HIMARK_SERVO_SUPPORT + const AP_Servo_Telem::TelemetryData telem_data { + .measured_position = ToDeg(msg.position), + .force = msg.force, + .speed = msg.speed, + .duty_cycle = msg.power_rating_pct, + .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION | + AP_Servo_Telem::TelemetryData::Types::FORCE | + AP_Servo_Telem::TelemetryData::Types::SPEED | + AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE + }; + + servo_telem->update_telem_data(msg.actuator_id, telem_data); +} +#endif + +#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED /* handle himark ServoInfo message */ void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg) { -#if HAL_LOGGING_ENABLED - // log as CSRV message - AP::logger().Write_ServoStatus(AP_HAL::micros64(), - msg.servo_id, - msg.pos_sensor*0.01, - 0, - 0, - 0, - msg.pos_cmd*0.01, - msg.voltage*0.01, - msg.current*0.01, - msg.motor_temp*0.2-40, - msg.pcb_temp*0.2-40, - msg.error_status); -#endif + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem == nullptr) { + return; + } + + const AP_Servo_Telem::TelemetryData telem_data { + .command_position = msg.pos_cmd * 0.01, + .measured_position = msg.pos_sensor * 0.01, + .voltage = msg.voltage * 0.01, + .current = msg.current * 0.01, + .motor_temperature_cdeg = int16_t(((msg.motor_temp * 0.2) - 40) * 100), + .pcb_temperature_cdeg = int16_t(((msg.pcb_temp * 0.2) - 40) * 100), + .status_flags = msg.error_status, + .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION | + AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION | + AP_Servo_Telem::TelemetryData::Types::VOLTAGE | + AP_Servo_Telem::TelemetryData::Types::CURRENT | + AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP | + AP_Servo_Telem::TelemetryData::Types::PCB_TEMP | + AP_Servo_Telem::TelemetryData::Types::STATUS + }; + + servo_telem->update_telem_data(msg.servo_id, telem_data); } #endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg) { -#if HAL_LOGGING_ENABLED - AP::logger().WriteStreaming( - "CVOL", - "TimeUS,Id,Pos,Cur,V,Pow,T", - "s#dAv%O", - "F-00000", - "QBfffBh", - AP_HAL::micros64(), - msg.actuator_id, - ToDeg(msg.actual_position), - msg.current * 0.025f, - msg.voltage * 0.2f, - uint8_t(msg.motor_pwm * (100.0/255.0)), - int16_t(msg.motor_temperature) - 50); -#endif + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem == nullptr) { + return; + } + + const AP_Servo_Telem::TelemetryData telem_data { + .measured_position = ToDeg(msg.actual_position), + .voltage = msg.voltage * 0.2, + .current = msg.current * 0.025, + .duty_cycle = msg.motor_pwm * (100.0/255.0), + .motor_temperature_cdeg = (int16_t(msg.motor_temperature) - 50)) * 100, + .valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION | + AP_Servo_Telem::TelemetryData::Types::VOLTAGE | + AP_Servo_Telem::TelemetryData::Types::CURRENT | + AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE | + AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP + }; + + servo_telem->update_telem_data(msg.actuator_id, telem_data); } #endif diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 9a7ae54c66..3e594bac07 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -36,6 +36,7 @@ #include #include #include +#include #ifndef DRONECAN_SRV_NUMBER #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS @@ -329,8 +330,10 @@ private: Canard::ObjCallback traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report}; Canard::Subscriber traffic_report_listener{traffic_report_cb, _driver_index}; +#if AP_SERVO_TELEM_ENABLED Canard::ObjCallback actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status}; Canard::Subscriber actuator_status_listener{actuator_status_cb, _driver_index}; +#endif Canard::ObjCallback esc_status_cb{this, &AP_DroneCAN::handle_ESC_status}; Canard::Subscriber esc_status_listener{esc_status_cb, _driver_index}; @@ -343,7 +346,11 @@ private: Canard::ObjCallback debug_cb{this, &AP_DroneCAN::handle_debug}; Canard::Subscriber debug_listener{debug_cb, _driver_index}; -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED +#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED + Canard::ObjCallback himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo}; + Canard::Subscriber himark_servo_ServoInfo_cb_listener{himark_servo_ServoInfo_cb, _driver_index}; +#endif +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED Canard::ObjCallback volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz}; Canard::Subscriber volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index}; #endif @@ -401,15 +408,19 @@ private: void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg); #endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT -#if AP_DRONECAN_HIMARK_SERVO_SUPPORT +#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg); #endif - + // incoming button handling void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg); void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg); +#if AP_SERVO_TELEM_ENABLED void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg); +#endif +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg); +#endif void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg); #if AP_EXTENDED_ESC_TELEM_ENABLED void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg); From 71137dac3152173476f993ccd415beb7fe1640eb Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:12:08 +0000 Subject: [PATCH 13/47] AP_PiccoloCAN: send incomming servo telem data to new `AP_Servo_Telem` lib --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 56 +++++++++++------------ libraries/AP_PiccoloCAN/AP_PiccoloCAN.h | 1 - 2 files changed, 26 insertions(+), 31 deletions(-) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 54c5453bfb..876b2149e3 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -31,10 +31,10 @@ #include #include #include -#include #include #include +#include #include @@ -320,8 +320,6 @@ bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint32_t timeout_us // called from SRV_Channels void AP_PiccoloCAN::update() { - uint64_t timestamp = AP_HAL::micros64(); - /* Read out the servo commands from the channel mixer */ for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) { @@ -361,46 +359,44 @@ void AP_PiccoloCAN::update() } #endif // AP_EFI_CURRAWONG_ECU_ENABLED -#if HAL_LOGGING_ENABLED - AP_Logger *logger = AP_Logger::get_singleton(); - - // Push received telemetry data into the logging system - if (logger && logger->logging_enabled()) { - - WITH_SEMAPHORE(_telem_sem); - +#if AP_SERVO_TELEM_ENABLED + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem != nullptr) { for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) { - AP_PiccoloCAN_Servo &servo = _servos[ii]; - if (servo.newTelemetry) { union { Servo_ErrorBits_t ebits; uint8_t errors; } err; err.ebits = servo.status.statusA.errors; - logger->Write_ServoStatus( - timestamp, - ii, - servo.position(), // Servo position (represented in microsecond units) - servo.current() * 0.01f, // Servo force (actually servo current, 0.01A per bit) - servo.speed(), // Servo speed (degrees per second) - servo.dutyCycle(), // Servo duty cycle (absolute value as it can be +/- 100%) - uint16_t(servo.commandedPosition()), // Commanded position - servo.voltage(), // Servo voltage - servo.current(), // Servo current - servo.temperature(), // Servo temperature - servo.temperature(), // - err.errors - ); + + const AP_Servo_Telem::TelemetryData telem_data { + .command_position = servo.commandedPosition(), + .measured_position = servo.position(), + .speed = servo.speed(), + .voltage = servo.voltage(), + .current = servo.current(), + .duty_cycle = servo.dutyCycle(), + .motor_temperature_cdeg = int16_t(servo.temperature() * 100), + .status_flags = err.errors, + .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION | + AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION | + AP_Servo_Telem::TelemetryData::Types::SPEED | + AP_Servo_Telem::TelemetryData::Types::VOLTAGE | + AP_Servo_Telem::TelemetryData::Types::CURRENT | + AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE | + AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP | + AP_Servo_Telem::TelemetryData::Types::STATUS + }; + + servo_telem->update_telem_data(ii, telem_data); servo.newTelemetry = false; } } } -#else - (void)timestamp; -#endif // HAL_LOGGING_ENABLED +#endif } diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 7353de1c03..71e5e44a75 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -131,7 +131,6 @@ private: AP_Int16 _ecu_id; //!< ECU Node ID AP_Int16 _ecu_hz; //!< ECU update rate (Hz) - HAL_Semaphore _telem_sem; }; #endif // HAL_PICCOLO_CAN_ENABLE From 9be1a751da7bb5a574408efb91beb8961f400650 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:12:45 +0000 Subject: [PATCH 14/47] AP_Volz_Protocol: send incomming servo telem data to new `AP_Servo_Telem` lib --- .../AP_Volz_Protocol/AP_Volz_Protocol.cpp | 70 ++++++++----------- libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 6 +- 2 files changed, 31 insertions(+), 45 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 4dac4792b3..c5318ef3ce 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include // Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default) #define PWM_POSITION_MIN 1000 @@ -77,7 +77,7 @@ void AP_Volz_Protocol::init(void) } } -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED // Request telem data, cycling through each servo and telem item void AP_Volz_Protocol::request_telem() { @@ -144,7 +144,7 @@ void AP_Volz_Protocol::loop() hal.scheduler->delay_microseconds(100); } -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED // Send a command for each servo, then one telem request const uint8_t num_servos = __builtin_popcount(bitmask.get()); if (sent_count < num_servos) { @@ -162,7 +162,7 @@ void AP_Volz_Protocol::loop() read_telem(); } -#else // No logging, send only +#else // No telem, send only send_position_cmd(); #endif } @@ -208,7 +208,7 @@ void AP_Volz_Protocol::send_position_cmd() send_command(cmd); -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED { // Update the commanded angle WITH_SEMAPHORE(telem.sem); @@ -221,7 +221,7 @@ void AP_Volz_Protocol::send_position_cmd() } } -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED void AP_Volz_Protocol::process_response(const CMD &cmd) { // Convert to 0 indexed @@ -348,7 +348,7 @@ void AP_Volz_Protocol::read_telem() // Used up all attempts without running out of data. // Really should not end up here } -#endif // HAL_LOGGING_ENABLED +#endif // AP_SERVO_TELEM_ENABLED // Called each time the servo outputs are sent void AP_Volz_Protocol::update() @@ -376,11 +376,11 @@ void AP_Volz_Protocol::update() servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm; } -#if HAL_LOGGING_ENABLED - // take semaphore and log all channels at 5 Hz - const uint32_t now_ms = AP_HAL::millis(); - if ((now_ms - telem.last_log_ms) > 200) { - telem.last_log_ms = now_ms; +#if AP_SERVO_TELEM_ENABLED + // Report telem data + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem != nullptr) { + const uint32_t now_ms = AP_HAL::millis(); WITH_SEMAPHORE(telem.sem); for (uint8_t i=0; iupdate_telem_data(i, telem_data); } } -#endif // HAL_LOGGING_ENABLED +#endif // AP_SERVO_TELEM_ENABLED } // Return the crc for a given command packet diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index f14df30230..d17fa76570 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -44,8 +44,7 @@ #include #include #include -#include - +#include class AP_Volz_Protocol { public: @@ -110,7 +109,7 @@ private: AP_Int16 range; bool initialised; -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED // Request telem data, cycling through each servo and telem item void request_telem(); @@ -143,7 +142,6 @@ private: int16_t motor_temp_deg; int16_t pcb_temp_deg; } data[NUM_SERVO_CHANNELS]; - uint32_t last_log_ms; } telem; #endif From d0bbc02995c4dccfcc2b095df54ed7825850e807 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:13:26 +0000 Subject: [PATCH 15/47] AP_Logger: remove `Write_ServoStatus` and `CSRV` definition --- libraries/AP_Logger/AP_Logger.h | 2 -- libraries/AP_Logger/LogFile.cpp | 25 -------------------- libraries/AP_Logger/LogStructure.h | 37 +++--------------------------- 3 files changed, 3 insertions(+), 61 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 9fe2c07f26..13dde72061 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -262,8 +262,6 @@ public: void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); - void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct, - float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error); void Write_Compass(); void Write_Mode(uint8_t mode, const ModeReason reason); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 4765af723c..fc100c0e94 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -452,31 +452,6 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason) return WriteCriticalBlock(&pkt, sizeof(pkt)); } -/* - write servo status from CAN servo - */ -void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct, - float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error) -{ - const struct log_CSRV pkt { - LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG), - time_us : time_us, - id : id, - position : position, - force : force, - speed : speed, - power_pct : power_pct, - pos_cmd : pos_cmd, - voltage : voltage, - current : current, - mot_temp : mot_temp, - pcb_temp : pcb_temp, - error : error, - }; - WriteBlock(&pkt, sizeof(pkt)); -} - - // Write a Yaw PID packet void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info) { diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 3b7c599bcd..418b8de1c8 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -148,6 +148,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include // structure used to define logging format // It is packed on ChibiOS to save flash space; however, this causes problems @@ -477,22 +478,6 @@ struct PACKED log_TERRAIN { float reference_offset; }; -struct PACKED log_CSRV { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t id; - float position; - float force; - float speed; - uint8_t power_pct; - float pos_cmd; - float voltage; - float current; - float mot_temp; - float pcb_temp; - uint8_t error; -}; - struct PACKED log_ARSP { LOG_PACKET_HEADER; uint64_t time_us; @@ -699,21 +684,6 @@ struct PACKED log_VER { // @Field: TR: innovation test ratio // @Field: Pri: True if sensor is the primary sensor -// @LoggerMessage: CSRV -// @Description: Servo feedback data -// @Field: TimeUS: Time since system startup -// @Field: Id: Servo number this data relates to -// @Field: Pos: Current servo position -// @Field: Force: Force being applied -// @Field: Speed: Current servo movement speed -// @Field: Pow: Amount of rated power being applied -// @Field: PosCmd: commanded servo position -// @Field: V: Voltage -// @Field: A: Current -// @Field: MotT: motor temperature -// @Field: PCBT: PCB temperature -// @Field: Err: error flags - // @LoggerMessage: DMS // @Description: DataFlash-Over-MAVLink statistics // @Field: TimeUS: Time since system startup @@ -1223,8 +1193,7 @@ LOG_STRUCTURE_FROM_AVOIDANCE \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","QBLLHffHHf","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs", "s-DU-mm--m", "F-GG-00--0", true }, \ LOG_STRUCTURE_FROM_ESC_TELEM \ - { LOG_CSRV_MSG, sizeof(log_CSRV), \ - "CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#---%dvAOO-", "F-000000000-", false }, \ +LOG_STRUCTURE_FROM_SERVO_TELEM \ { LOG_PIDR_MSG, sizeof(log_PID), \ "PIDR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS, true }, \ { LOG_PIDP_MSG, sizeof(log_PID), \ @@ -1307,7 +1276,7 @@ enum LogMessages : uint8_t { LOG_IDS_FROM_CAMERA, LOG_IDS_FROM_MOUNT, LOG_TERRAIN_MSG, - LOG_CSRV_MSG, + LOG_IDS_FROM_SERVO_TELEM, LOG_IDS_FROM_ESC_TELEM, LOG_IDS_FROM_BATTMONITOR, LOG_IDS_FROM_HAL_CHIBIOS, From e003cc511dc16f594f90ec774f766df548115674 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:14:01 +0000 Subject: [PATCH 16/47] add `AP_Servo_Telem` --- libraries/AP_Servo_Telem/AP_Servo_Telem.cpp | 160 ++++++++++++++++++ libraries/AP_Servo_Telem/AP_Servo_Telem.h | 84 +++++++++ .../AP_Servo_Telem/AP_Servo_Telem_config.h | 8 + libraries/AP_Servo_Telem/LogStructure.h | 47 +++++ 4 files changed, 299 insertions(+) create mode 100644 libraries/AP_Servo_Telem/AP_Servo_Telem.cpp create mode 100644 libraries/AP_Servo_Telem/AP_Servo_Telem.h create mode 100644 libraries/AP_Servo_Telem/AP_Servo_Telem_config.h create mode 100644 libraries/AP_Servo_Telem/LogStructure.h diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp new file mode 100644 index 0000000000..923b5bd261 --- /dev/null +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp @@ -0,0 +1,160 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_Servo_Telem.h" + +#if AP_SERVO_TELEM_ENABLED + +#include +#include +#include + +AP_Servo_Telem *AP_Servo_Telem::_singleton; + +AP_Servo_Telem::AP_Servo_Telem() +{ + if (_singleton) { + AP_HAL::panic("Too many AP_Servo_Telem instances"); + } + _singleton = this; +} + +// return true if the data is stale +bool AP_Servo_Telem::TelemetryData::stale(uint32_t now_ms) const volatile +{ + return (last_update_ms == 0) || ((now_ms - last_update_ms) > 5000); +} + +// return true if the requested types of data are available +bool AP_Servo_Telem::TelemetryData::present(const uint16_t type_mask) const volatile +{ + return (valid_types & type_mask) != 0; +} + +// return true if the requested types of data are available and not stale +bool AP_Servo_Telem::TelemetryData::valid(const uint16_t type_mask) const volatile +{ + return present(type_mask) && !stale(AP_HAL::millis()); +} + +// record an update to the telemetry data together with timestamp +// callback to update the data in the frontend, should be called by the driver when new data is available +void AP_Servo_Telem::update_telem_data(const uint8_t servo_index, const TelemetryData& new_data) +{ + // telemetry data are not protected by a semaphore even though updated from different threads + // each element is a primitive type and the timestamp is only updated at the end, thus a caller + // can only get slightly more up-to-date information that perhaps they were expecting or might + // read data that has just gone stale - both of these are safe and avoid the overhead of locking + + if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.valid_types == 0)) { + return; + } + + volatile TelemetryData &telemdata = _telem_data[servo_index]; + + if (new_data.present(TelemetryData::Types::COMMANDED_POSITION)) { + telemdata.command_position = new_data.command_position; + } + if (new_data.present(TelemetryData::Types::MEASURED_POSITION)) { + telemdata.measured_position = new_data.measured_position; + } + if (new_data.present(TelemetryData::Types::FORCE)) { + telemdata.force = new_data.force; + } + if (new_data.present(TelemetryData::Types::SPEED)) { + telemdata.speed = new_data.speed; + } + if (new_data.present(TelemetryData::Types::VOLTAGE)) { + telemdata.voltage = new_data.voltage; + } + if (new_data.present(TelemetryData::Types::CURRENT)) { + telemdata.current = new_data.current; + } + if (new_data.present(TelemetryData::Types::DUTY_CYCLE)) { + telemdata.duty_cycle = new_data.duty_cycle; + } + if (new_data.present(TelemetryData::Types::MOTOR_TEMP)) { + telemdata.motor_temperature_cdeg = new_data.motor_temperature_cdeg; + } + if (new_data.present(TelemetryData::Types::PCB_TEMP)) { + telemdata.pcb_temperature_cdeg = new_data.pcb_temperature_cdeg; + } + if (new_data.present(TelemetryData::Types::PCB_TEMP)) { + telemdata.status_flags = new_data.status_flags; + } + + telemdata.valid_types |= new_data.valid_types; + telemdata.last_update_ms = AP_HAL::millis(); +} + +void AP_Servo_Telem::update() +{ +#if HAL_LOGGING_ENABLED + write_log(); +#endif +} + +#if HAL_LOGGING_ENABLED +void AP_Servo_Telem::write_log() +{ + AP_Logger *logger = AP_Logger::get_singleton(); + + // Check logging is available and enabled + if ((logger == nullptr) || !logger->logging_enabled()) { + return; + } + + const uint64_t now_us = AP_HAL::micros64(); + + for (uint8_t i = 0; i < ARRAY_SIZE(_telem_data); i++) { + const volatile TelemetryData &telemdata = _telem_data[i]; + + if (telemdata.last_update_ms == _last_telem_log_ms[i]) { + // No new data since last log call, skip + continue; + } + + // Update last log timestamp + _last_telem_log_ms[i] = telemdata.last_update_ms; + + // Log, use nan for float values which are not available + const struct log_CSRV pkt { + LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG), + time_us : now_us, + id : i, + position : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position : AP::logger().quiet_nanf(), + force : telemdata.present(TelemetryData::Types::FORCE) ? telemdata.force : AP::logger().quiet_nanf(), + speed : telemdata.present(TelemetryData::Types::SPEED) ? telemdata.speed : AP::logger().quiet_nanf(), + power_pct : telemdata.duty_cycle, + pos_cmd : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.command_position : AP::logger().quiet_nanf(), + voltage : telemdata.present(TelemetryData::Types::VOLTAGE) ? telemdata.voltage : AP::logger().quiet_nanf(), + current : telemdata.present(TelemetryData::Types::CURRENT) ? telemdata.current : AP::logger().quiet_nanf(), + mot_temp : telemdata.present(TelemetryData::Types::MOTOR_TEMP) ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), + pcb_temp : telemdata.present(TelemetryData::Types::PCB_TEMP) ? telemdata.pcb_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), + error : telemdata.status_flags, + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + } + +} +#endif // HAL_LOGGING_ENABLED + +// Get the AP_Servo_Telem singleton +AP_Servo_Telem *AP_Servo_Telem::get_singleton() +{ + return AP_Servo_Telem::_singleton; +} + +#endif // AP_SERVO_TELEM_ENABLED diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.h b/libraries/AP_Servo_Telem/AP_Servo_Telem.h new file mode 100644 index 0000000000..bff0aece82 --- /dev/null +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.h @@ -0,0 +1,84 @@ +#pragma once + +#include "AP_Servo_Telem_config.h" + +#if AP_SERVO_TELEM_ENABLED + +#include +#include + + +#ifndef SERVO_TELEM_MAX_SERVOS + #define SERVO_TELEM_MAX_SERVOS NUM_SERVO_CHANNELS +#endif +static_assert(SERVO_TELEM_MAX_SERVOS > 0, "Cannot have 0 Servo telem instances"); + +class AP_Servo_Telem { +public: + AP_Servo_Telem(); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Servo_Telem); + + static AP_Servo_Telem *get_singleton(); + + struct TelemetryData { + // Telemetry values + float command_position; // Commanded servo position in degrees + float measured_position; // Measured Servo position in degrees + float force; // Force in Newton meters + float speed; // Speed degrees per second + float voltage; // Voltage in volts + float current; // Current draw in Ampere + uint8_t duty_cycle; // duty cycle 0% to 100% + int16_t motor_temperature_cdeg; // centi-degrees C, negative values allowed + int16_t pcb_temperature_cdeg; // centi-degrees C, negative values allowed + uint8_t status_flags; // Type specific status flags + + // last update time in milliseconds, determines data is stale + uint32_t last_update_ms; + + // telemetry types present + enum Types { + COMMANDED_POSITION = 1 << 0, + MEASURED_POSITION = 1 << 1, + FORCE = 1 << 2, + SPEED = 1 << 3, + VOLTAGE = 1 << 4, + CURRENT = 1 << 5, + DUTY_CYCLE = 1 << 6, + MOTOR_TEMP = 1 << 7, + PCB_TEMP = 1 << 8, + STATUS = 1 << 9, + }; + uint16_t valid_types; + + // return true if the data is stale + bool stale(uint32_t now_ms) const volatile; + + // return true if the requested types of data are available + bool present(const uint16_t type_mask) const volatile; + + // return true if the requested type of data is available and not stale + bool valid(const uint16_t type_mask) const volatile; + }; + + // update at 10Hz to log telemetry + void update(); + + // record an update to the telemetry data together with timestamp + // callback to update the data in the frontend, should be called by the driver when new data is available + void update_telem_data(const uint8_t servo_index, const TelemetryData& new_data); + +private: + + // Log telem of each servo + void write_log(); + + volatile TelemetryData _telem_data[SERVO_TELEM_MAX_SERVOS]; + + uint32_t _last_telem_log_ms[SERVO_TELEM_MAX_SERVOS]; + + static AP_Servo_Telem *_singleton; +}; +#endif // AP_SERVO_TELEM_ENABLED diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem_config.h b/libraries/AP_Servo_Telem/AP_Servo_Telem_config.h new file mode 100644 index 0000000000..97836c5366 --- /dev/null +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem_config.h @@ -0,0 +1,8 @@ +#pragma once + +#include +#include + +#ifndef AP_SERVO_TELEM_ENABLED +#define AP_SERVO_TELEM_ENABLED ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS)) && !defined(HAL_BUILD_AP_PERIPH)) +#endif diff --git a/libraries/AP_Servo_Telem/LogStructure.h b/libraries/AP_Servo_Telem/LogStructure.h new file mode 100644 index 0000000000..d8dcb9771a --- /dev/null +++ b/libraries/AP_Servo_Telem/LogStructure.h @@ -0,0 +1,47 @@ +#pragma once + +#include "AP_Servo_Telem_config.h" +#include + +#define LOG_IDS_FROM_SERVO_TELEM \ + LOG_CSRV_MSG + +// @LoggerMessage: CSRV +// @Description: Servo feedback data +// @Field: TimeUS: Time since system startup +// @Field: Id: Servo number this data relates to +// @Field: Pos: Current servo position +// @Field: Force: Force being applied +// @Field: Speed: Current servo movement speed +// @Field: Pow: Amount of rated power being applied +// @Field: PosCmd: commanded servo position +// @Field: V: Voltage +// @Field: A: Current +// @Field: MotT: motor temperature +// @Field: PCBT: PCB temperature +// @Field: Err: error flags + +struct PACKED log_CSRV { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t id; + float position; + float force; + float speed; + uint8_t power_pct; + float pos_cmd; + float voltage; + float current; + float mot_temp; + float pcb_temp; + uint8_t error; +}; + + +#if !AP_SERVO_TELEM_ENABLED +#define LOG_STRUCTURE_FROM_SERVO_TELEM +#else +#define LOG_STRUCTURE_FROM_SERVO_TELEM \ + { LOG_CSRV_MSG, sizeof(log_CSRV), \ + "CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#dtk%dvAOO-", "F-000000000-", true }, +#endif From 38c32571516e36b8ff07dde7cbfbc1514abccae5 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:14:34 +0000 Subject: [PATCH 17/47] AP_Vechicle: add `AP_Servo_Telem` and call at 50Hz --- libraries/AP_Vehicle/AP_Vehicle.cpp | 3 +++ libraries/AP_Vehicle/AP_Vehicle.h | 5 +++++ 2 files changed, 8 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 3005e40d3a..b79373b364 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -634,6 +634,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #if HAL_WITH_ESC_TELEM SCHED_TASK_CLASS(AP_ESC_Telem, &vehicle.esc_telem, update, 100, 50, 230), #endif +#if AP_SERVO_TELEM_ENABLED + SCHED_TASK_CLASS(AP_Servo_Telem, &vehicle.servo_telem, update, 50, 50, 231), +#endif #if HAL_GENERATOR_ENABLED SCHED_TASK_CLASS(AP_Generator, &vehicle.generator, update, 10, 50, 235), #endif diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index f56262e0c3..cba506cb9b 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -415,6 +416,10 @@ protected: AP_ESC_Telem esc_telem; #endif +#if AP_SERVO_TELEM_ENABLED + AP_Servo_Telem servo_telem; +#endif + #if AP_OPENDRONEID_ENABLED AP_OpenDroneID opendroneid; #endif From b9dd814935be17fdc8ea11fdff0d81774532340b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:15:14 +0000 Subject: [PATCH 18/47] Tools: ardupilotwaf: add `AP_Servo_Telem` to common vehicle libraries --- Tools/ardupilotwaf/ardupilotwaf.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 880d1978df..05e42c5eb4 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -107,6 +107,7 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [ 'AP_EFI', 'AP_Hott_Telem', 'AP_ESC_Telem', + 'AP_Servo_Telem', 'AP_Stats', 'AP_GyroFFT', 'AP_RCTelemetry', From a47aaef2b4cfb03fdc2fcd5a522e79b4162ea982 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 12:48:20 +0000 Subject: [PATCH 19/47] Tools: scripts: build options and extract features: add `AP_Servo_Telem` --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index b140fe125b..902f77fb2a 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -406,6 +406,7 @@ BUILD_OPTIONS = [ Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'Enable SDP3X AIRSPEED', 0, 'AIRSPEED'), Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'Enable DroneCAN AIRSPEED', 0, 'AIRSPEED,DroneCAN'), # NOQA: E501 + Feature('Actuators', 'Servo telem', 'AP_SERVO_TELEM_ENABLED', 'Enable servo telemetry library', 0, None), Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None), Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, "DroneCAN,Volz"), # noqa: E501 Feature('Actuators', 'RobotisServo', 'AP_ROBOTISSERVO_ENABLED', 'Enable RobotisServo protocol', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index c4a554e805..6e51e333e8 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -190,6 +190,7 @@ class ExtractFeatures(object): ('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P.*)::_process_byte\b',), ('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P.*)::process_pulse\b',), + ('AP_SERVO_TELEM_ENABLED', r'AP_Servo_Telem::update\b',), ('AP_VOLZ_ENABLED', r'AP_Volz_Protocol::init\b',), ('AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', r'AP_DroneCAN::handle_actuator_status_Volz\b',), ('AP_ROBOTISSERVO_ENABLED', r'AP_RobotisServo::init\b',), From 13807f24d73989cfd31644578cfb83cfe9853f2b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Nov 2024 08:04:15 +1100 Subject: [PATCH 20/47] AP_Periph: added simulation of DroneCAN servo status allows for testing of DroneCAN servo logging in SITL --- Tools/AP_Periph/AP_Periph.h | 9 +++++++ Tools/AP_Periph/rc_out.cpp | 51 +++++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+) diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 0cbf5f419e..a3694e122c 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -568,6 +568,15 @@ public: uint16_t pool_peak_percent(); void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue); +#if AP_SIM_ENABLED + // update simulation of servos + void sim_update_actuator(uint8_t actuator_id); + struct { + uint32_t mask; + uint32_t last_send_ms; + } sim_actuator; +#endif + struct dronecan_protocol_t { CanardInstance canard; uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)]; diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 8feb3e0805..2d8df1079a 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -15,6 +15,9 @@ #include #ifdef HAL_PERIPH_ENABLE_RC_OUT #include "AP_Periph.h" +#if AP_SIM_ENABLED +#include +#endif // magic value from UAVCAN driver packet // dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan @@ -112,6 +115,9 @@ void AP_Periph_FW::rcout_srv_unitless(uint8_t actuator_id, const float command_v SRV_Channels::set_output_norm(function, command_value); rcout_has_new_data_to_update = true; +#if AP_SIM_ENABLED + sim_update_actuator(actuator_id); +#endif #endif } @@ -122,6 +128,9 @@ void AP_Periph_FW::rcout_srv_PWM(uint8_t actuator_id, const float command_value) SRV_Channels::set_output_pwm(function, uint16_t(command_value+0.5)); rcout_has_new_data_to_update = true; +#if AP_SIM_ENABLED + sim_update_actuator(actuator_id); +#endif #endif } @@ -172,4 +181,46 @@ void AP_Periph_FW::rcout_update() #endif } +#if AP_SIM_ENABLED +/* + update simulation of servos, sending actuator status +*/ +void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id) +{ + sim_actuator.mask |= 1U << actuator_id; + + // send status at 10Hz + const uint32_t period_ms = 100; + const uint32_t now_ms = AP_HAL::millis(); + + if (now_ms - sim_actuator.last_send_ms < period_ms) { + return; + } + sim_actuator.last_send_ms = now_ms; + + for (uint8_t i=0; i Date: Tue, 19 Nov 2024 08:19:37 +1100 Subject: [PATCH 21/47] AP_DroneCAN: actuator status is measured, not commanded --- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 8f07785773..58cdc7e1b6 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1393,7 +1393,7 @@ void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const .force = msg.force, .speed = msg.speed, .duty_cycle = msg.power_rating_pct, - .valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION | + .valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION | AP_Servo_Telem::TelemetryData::Types::FORCE | AP_Servo_Telem::TelemetryData::Types::SPEED | AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE From 445c03c69cdd6b71d4b770b9d5ca6df4bfabf510 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Nov 2024 08:19:58 +1100 Subject: [PATCH 22/47] AP_ServoTelem: added active mask and fixed typo in logging --- libraries/AP_Servo_Telem/AP_Servo_Telem.cpp | 19 ++++++++++--------- libraries/AP_Servo_Telem/AP_Servo_Telem.h | 2 ++ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp index 923b5bd261..3b957b11f2 100644 --- a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp @@ -61,6 +61,7 @@ void AP_Servo_Telem::update_telem_data(const uint8_t servo_index, const Telemetr if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.valid_types == 0)) { return; } + active_mask |= 1U << servo_index; volatile TelemetryData &telemdata = _telem_data[servo_index]; @@ -112,7 +113,7 @@ void AP_Servo_Telem::write_log() AP_Logger *logger = AP_Logger::get_singleton(); // Check logging is available and enabled - if ((logger == nullptr) || !logger->logging_enabled()) { + if ((logger == nullptr) || !logger->logging_enabled() || active_mask == 0) { return; } @@ -134,15 +135,15 @@ void AP_Servo_Telem::write_log() LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG), time_us : now_us, id : i, - position : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position : AP::logger().quiet_nanf(), - force : telemdata.present(TelemetryData::Types::FORCE) ? telemdata.force : AP::logger().quiet_nanf(), - speed : telemdata.present(TelemetryData::Types::SPEED) ? telemdata.speed : AP::logger().quiet_nanf(), + position : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position : AP::logger().quiet_nanf(), + force : telemdata.present(TelemetryData::Types::FORCE) ? telemdata.force : AP::logger().quiet_nanf(), + speed : telemdata.present(TelemetryData::Types::SPEED) ? telemdata.speed : AP::logger().quiet_nanf(), power_pct : telemdata.duty_cycle, - pos_cmd : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.command_position : AP::logger().quiet_nanf(), - voltage : telemdata.present(TelemetryData::Types::VOLTAGE) ? telemdata.voltage : AP::logger().quiet_nanf(), - current : telemdata.present(TelemetryData::Types::CURRENT) ? telemdata.current : AP::logger().quiet_nanf(), - mot_temp : telemdata.present(TelemetryData::Types::MOTOR_TEMP) ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), - pcb_temp : telemdata.present(TelemetryData::Types::PCB_TEMP) ? telemdata.pcb_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), + pos_cmd : telemdata.present(TelemetryData::Types::COMMANDED_POSITION) ? telemdata.command_position : AP::logger().quiet_nanf(), + voltage : telemdata.present(TelemetryData::Types::VOLTAGE) ? telemdata.voltage : AP::logger().quiet_nanf(), + current : telemdata.present(TelemetryData::Types::CURRENT) ? telemdata.current : AP::logger().quiet_nanf(), + mot_temp : telemdata.present(TelemetryData::Types::MOTOR_TEMP) ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), + pcb_temp : telemdata.present(TelemetryData::Types::PCB_TEMP) ? telemdata.pcb_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(), error : telemdata.status_flags, }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.h b/libraries/AP_Servo_Telem/AP_Servo_Telem.h index bff0aece82..ad3e97cab6 100644 --- a/libraries/AP_Servo_Telem/AP_Servo_Telem.h +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.h @@ -80,5 +80,7 @@ private: uint32_t _last_telem_log_ms[SERVO_TELEM_MAX_SERVOS]; static AP_Servo_Telem *_singleton; + + uint32_t active_mask; }; #endif // AP_SERVO_TELEM_ENABLED From a79fcdbfcdd9f21792acea39c72474e95bc80037 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 10 Nov 2024 11:56:58 +0000 Subject: [PATCH 23/47] Plane: support DO_RETURN_PATH_START misison item and command --- ArduPlane/AP_Arming.cpp | 12 +++++++++--- ArduPlane/GCS_Mavlink.cpp | 20 ++++++++++++++++++-- ArduPlane/Parameters.cpp | 2 +- ArduPlane/commands_logic.cpp | 2 ++ ArduPlane/defines.h | 2 +- ArduPlane/mode_rtl.cpp | 17 +++++++++++++++-- 6 files changed, 46 insertions(+), 9 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 643f93a6c1..fca35f8d08 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -433,9 +433,15 @@ bool AP_Arming_Plane::mission_checks(bool report) { // base checks bool ret = AP_Arming::mission_checks(report); - if (plane.mission.contains_item(MAV_CMD_DO_LAND_START) && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) { - ret = false; - check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled"); + if (plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) { + if (plane.mission.contains_item(MAV_CMD_DO_LAND_START)) { + ret = false; + check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled"); + } + if (plane.mission.contains_item(MAV_CMD_DO_RETURN_PATH_START)) { + ret = false; + check_failed(ARMING_CHECK_MISSION, report, "DO_RETURN_PATH_START set and RTL_AUTOLAND disabled"); + } } #if HAL_QUADPLANE_ENABLED if (plane.quadplane.available()) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 298b4e47a5..7aaba22f70 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1072,11 +1072,27 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_DO_GO_AROUND: return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; + case MAV_CMD_DO_RETURN_PATH_START: + // attempt to rejoin after the next DO_RETURN_PATH_START command in the mission + if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) { + plane.mission.set_force_resume(true); + if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + // mode change failed, revert force resume flag + plane.mission.set_force_resume(false); + } + return MAV_RESULT_FAILED; + case MAV_CMD_DO_LAND_START: // attempt to switch to next DO_LAND_START command in the mission if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) { - plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); - return MAV_RESULT_ACCEPTED; + plane.mission.set_force_resume(true); + if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_ACCEPTED; + } + // mode change failed, revert force resume flag + plane.mission.set_force_resume(false); } return MAV_RESULT_FAILED; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 04a5ba3a59..8dcd235220 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -746,7 +746,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: RTL_AUTOLAND // @DisplayName: RTL auto land // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. If this is set to 0 and there is a DO_LAND_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around without it changing RTL behaviour. For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation). - // @Values: 0:Disable,1:Fly HOME then land,2:Go directly to landing sequence, 3:OnlyForGoAround + // @Values: 0:Disable,1:Fly HOME then land via DO_LAND_START mission item, 2:Go directly to landing sequence via DO_LAND_START mission item, 3:OnlyForGoAround, 4:Go directly to landing sequence via DO_RETURN_PATH_START mission item // @User: Standard GSCALAR(rtl_autoland, "RTL_AUTOLAND", float(RtlAutoland::RTL_DISABLE)), diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 90c3e02480..2110b1455d 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -138,6 +138,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) } break; + case MAV_CMD_DO_RETURN_PATH_START: case MAV_CMD_DO_LAND_START: break; @@ -304,6 +305,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_HOME: case MAV_CMD_DO_INVERTED_FLIGHT: + case MAV_CMD_DO_RETURN_PATH_START: case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_FENCE_ENABLE: case MAV_CMD_DO_AUTOTUNE_ENABLE: diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a3da572611..561e1deb39 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -61,8 +61,8 @@ enum class RtlAutoland { RTL_THEN_DO_LAND_START = 1, RTL_IMMEDIATE_DO_LAND_START = 2, NO_RTL_GO_AROUND = 3, + DO_RETURN_PATH_START = 4, }; - // PID broadcast bitmask enum tuning_pid_bits { diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index b0d8cfbfd6..a6f2fef892 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -106,8 +106,7 @@ void ModeRTL::navigate() if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) || (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START && plane.reached_loiter_target() && - labs(plane.calc_altitude_error_cm()) < 1000)) - { + labs(plane.calc_altitude_error_cm()) < 1000)) { // we've reached the RTL point, see if we have a landing sequence if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) { // switch from RTL -> AUTO @@ -116,12 +115,26 @@ void ModeRTL::navigate() // return here so we don't change the radius and don't run the rtl update_loiter() return; } + // mode change failed, revert force resume flag + plane.mission.set_force_resume(false); } // prevent running the expensive jump_to_landing_sequence // on every loop plane.auto_state.checked_for_autoland = true; + + } else if (plane.g.rtl_autoland == RtlAutoland::DO_RETURN_PATH_START) { + if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) { + plane.mission.set_force_resume(true); + if (plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND)) { + // return here so we don't change the radius and don't run the rtl update_loiter() + return; + } + // mode change failed, revert force resume flag + plane.mission.set_force_resume(false); } + plane.auto_state.checked_for_autoland = true; + } } } From eaf20db6ea635eabfc1a8b85d5dca63399a174e6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 29 Nov 2024 10:30:29 +0900 Subject: [PATCH 24/47] AP_Scripting: ahrs/ekf origin script promoted to an applet --- .../AP_Scripting/applets/ahrs-set-origin.lua | 100 ++++++++++++++++++ .../AP_Scripting/applets/ahrs-set-origin.md | 15 +++ .../AP_Scripting/examples/ahrs-set-origin.lua | 23 ---- 3 files changed, 115 insertions(+), 23 deletions(-) create mode 100644 libraries/AP_Scripting/applets/ahrs-set-origin.lua create mode 100644 libraries/AP_Scripting/applets/ahrs-set-origin.md delete mode 100644 libraries/AP_Scripting/examples/ahrs-set-origin.lua diff --git a/libraries/AP_Scripting/applets/ahrs-set-origin.lua b/libraries/AP_Scripting/applets/ahrs-set-origin.lua new file mode 100644 index 0000000000..a2f25fda87 --- /dev/null +++ b/libraries/AP_Scripting/applets/ahrs-set-origin.lua @@ -0,0 +1,100 @@ +-- Sets the AHRS/EKF origin to a specified Location +-- +-- Parameter descriptions +-- AHRS_ORIG_LAT : AHRS Origin Latitude (in degrees) +-- AHRS_ORIG_LON : AHRS Origin Longitude (in degrees) +-- AHRS_ORIGIN_ALT : AHRS Origin Altitude (in meters above sea level) +-- +-- How to use +-- 1. Install this script on the flight controller +-- 2. Set AHRS_ORIG_LAT, AHRS_ORIG_LON, AHRS_ORIG_ALT to the desired location +-- 3. A message should appear on the messages screen when the AHRS/EKF origin has been set and the vehicle will most often then appear on the map + +local PARAM_TABLE_KEY = 87 +PARAM_TABLE_PREFIX = "AHRS_ORIG_" +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local SEND_TEXT_PREFIX = "ahrs-set-origin: " + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- add param table +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), SEND_TEXT_PREFIX .. 'could not add param table') + +--[[ + // @Param: AHRS_ORIG_LAT + // @DisplayName: AHRS/EKF Origin Latitude + // @Description: AHRS/EKF origin will be set to this latitude if not already set + // @Range: -180 180 + // @User: Standard +--]] +local AHRS_ORIG_LAT = bind_add_param('LAT', 1, 0) + +--[[ + // @Param: AHRS_ORIG_LON + // @DisplayName: AHRS/EKF Origin Longitude + // @Description: AHRS/EKF origin will be set to this longitude if not already set + // @Range: -180 180 + // @User: Standard +--]] +local AHRS_ORIG_LON = bind_add_param('LON', 2, 0) + +--[[ + // @Param: AHRS_ORIG_ALT + // @DisplayName: AHRS/EKF Origin Altitude + // @Description: AHRS/EKF origin will be set to this altitude (in meters above sea level) if not already set + // @Range: 0 10000 + // @User: Standard +--]] +local AHRS_ORIG_ALT = bind_add_param('ALT', 3, 0) + +-- print welcome message +gcs:send_text(MAV_SEVERITY.INFO, SEND_TEXT_PREFIX .. "started") + +function update() + + -- wait for AHRS to be initialised + if not ahrs:initialised() then + return update, 5000 + end + + -- exit if AHRS/EKF origin has already been set + if ahrs:get_origin() then + gcs:send_text(MAV_SEVERITY.WARNING, SEND_TEXT_PREFIX .. "EKF origin already set") + return + end + + -- return if parameters have not been set + if AHRS_ORIG_LAT:get() == 0 and AHRS_ORIG_LON:get() == 0 and AHRS_ORIG_ALT == 0 then + -- try again in 5 seconds + return update, 5000 + end + + -- create new origin + local location = Location() + location:lat(math.floor(AHRS_ORIG_LAT:get() * 10000000.0)) + location:lng(math.floor(AHRS_ORIG_LON:get() * 10000000.0)) + location:alt(math.floor(AHRS_ORIG_ALT:get() * 100.0)) + + -- attempt to send origin + if ahrs:set_origin(location) then + gcs:send_text(MAV_SEVERITY.INFO, string.format(SEND_TEXT_PREFIX .. "origin set Lat:%.7f Lon:%.7f Alt:%.1f", AHRS_ORIG_LAT:get(), AHRS_ORIG_LON:get(), AHRS_ORIG_ALT:get())) + else + gcs:send_text(MAV_SEVERITY.WARNING, SEND_TEXT_PREFIX .. "failed to set origin") + end + + -- return and do not try again + return +end + +return update() \ No newline at end of file diff --git a/libraries/AP_Scripting/applets/ahrs-set-origin.md b/libraries/AP_Scripting/applets/ahrs-set-origin.md new file mode 100644 index 0000000000..27ab6fc5c6 --- /dev/null +++ b/libraries/AP_Scripting/applets/ahrs-set-origin.md @@ -0,0 +1,15 @@ +# AHRS set origin + +Sets the AHRS/EKF origin to a specified Location + +## Parmeter Descriptions + +-- AHRS_ORIG_LAT : AHRS Origin Latitude (in degrees) +-- AHRS_ORIG_LON : AHRS Origin Longitude (in degrees) +-- AHRS_ORIGIN_ALT : AHRS Origin Altitude (in meters above sea level) + +## How to use + +Install this script on the flight controller +Set AHRS_ORIG_LAT, AHRS_ORIG_LON, AHRS_ORIG_ALT to the desired location +A message should appear on the messages screen when the AHRS/EKF origin has been set and the vehicle will most often then appear on the map diff --git a/libraries/AP_Scripting/examples/ahrs-set-origin.lua b/libraries/AP_Scripting/examples/ahrs-set-origin.lua deleted file mode 100644 index 37ec058d2c..0000000000 --- a/libraries/AP_Scripting/examples/ahrs-set-origin.lua +++ /dev/null @@ -1,23 +0,0 @@ --- example script for using "set_origin()"" and "initialised()" --- sets the ekf origin if not already set - - -function update () - - if not ahrs:initialised() then - return update, 5000 - end - - origin = assert(not ahrs:get_origin(),"Refused to set EKF origin - already set") - location = Location() location:lat(-353632640) location:lng(1491652352) location:alt(58409) - - if ahrs:set_origin(location) then - gcs:send_text(6, string.format("Origin Set - Lat:%.7f Long:%.7f Alt:%.1f", location:lat()/10000000, location:lng()/10000000, location:alt()/100)) - else - gcs:send_text(0, "Refused to set EKF origin") - end - - return -end - -return update() \ No newline at end of file From f84c855dd122084c5708fee1274ac3bec30468ea Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 11 Feb 2024 16:19:56 +1100 Subject: [PATCH 25/47] Copter: run copter attitude control with separate rate thread run motors output at rate thread loop rate allow rate thread to be enabled/disabled at runtime for in-flight impact testing setup the right PID notch sample rate when using the rate thread the PID notches run at a very different sample rate call update_dynamic_notch_at_specified_rate() in rate thread log RTDT messages to track rate loop performance set dt each cycle of the rate loop thread run rate controller on samples as soon as they are ready detect overload conditions in both the rate loop and main loop decimate the rate thread if the CPU appears overloaded decimate the gyro window inside the IMU add in gyro drift to attitude rate thread add fixed-rate thread option configure rate loop based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED better rate loop thread decimation management ensure fix rate attitude is enabled on arming add rate loop timing debug update backend filters rather than all the backends provide more options around attitude rates only log attitude and IMU from main loop force trigger_groups() and reduce attitude thread priority migrate fast rate enablement to FSTRATE_ENABLE remove rate thread logging configuration and choose sensible logging rates conditionally compile rate thread pieces allow fast rate decimation to be user throttled if target rate changes immediately jump to target rate recover quickly from rate changes ensure fixed rate always prints the rate on arming and is always up to date add support for fixed rate attitude that does not change when disarmed only push to subsystems at main loop rate add logging and motor timing debug correctly round gyro decimation rates set dshot rate when changing attitude rate fallback to higher dshot rates at lower loop rates re-factor rate loop rate updates log rates in systemid mode reset target modifiers at loop rate don't compile in support on tradheli move rate thread into its own compilation unit add rate loop config abstraction that allows code to be elided on non-copter builds dynamically enable/disable rate thread correctly add design comment for the rate thread Co-authored-by: Andrew Tridgell --- ArduCopter/Attitude.cpp | 17 +- ArduCopter/Copter.cpp | 39 ++- ArduCopter/Copter.h | 31 ++- ArduCopter/Log.cpp | 4 + ArduCopter/Parameters.cpp | 17 ++ ArduCopter/Parameters.h | 3 + ArduCopter/mode_systemid.cpp | 1 + ArduCopter/motor_test.cpp | 6 + ArduCopter/motors.cpp | 16 +- ArduCopter/rate_thread.cpp | 486 +++++++++++++++++++++++++++++++++++ 10 files changed, 602 insertions(+), 18 deletions(-) create mode 100644 ArduCopter/rate_thread.cpp diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 3e71782c18..02bbee2749 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -4,18 +4,21 @@ * Attitude Rate controllers and timing ****************************************************************/ -// update rate controllers and output to roll, pitch and yaw actuators -// called at 400hz by default -void Copter::run_rate_controller() +/* + update rate controller when run from main thread (normal operation) +*/ +void Copter::run_rate_controller_main() { // set attitude and position controller loop time const float last_loop_time_s = AP::scheduler().get_last_loop_time_s(); - motors->set_dt(last_loop_time_s); - attitude_control->set_dt(last_loop_time_s); pos_control->set_dt(last_loop_time_s); + attitude_control->set_dt(last_loop_time_s); - // run low level rate controllers that only require IMU data - attitude_control->rate_controller_run(); + if (!using_rate_thread) { + motors->set_dt(last_loop_time_s); + // only run the rate controller if we are not using the rate thread + attitude_control->rate_controller_run(); + } // reset sysid and other temporary inputs attitude_control->rate_controller_target_reset(); } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 0f263b89c0..a3ac456c8a 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -75,6 +75,7 @@ */ #include "Copter.h" +#include #define FORCE_VERSION_H_INCLUDE #include "version.h" @@ -113,7 +114,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { // update INS immediately to get current gyro data populated FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update), // run low level rate controllers that only require IMU data - FAST_TASK(run_rate_controller), + FAST_TASK(run_rate_controller_main), #if AC_CUSTOMCONTROL_MULTI_ENABLED FAST_TASK(run_custom_controller), #endif @@ -121,7 +122,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { FAST_TASK(heli_update_autorotation), #endif //HELI_FRAME // send outputs to the motors library immediately - FAST_TASK(motors_output), + FAST_TASK(motors_output_main), // run EKF state estimator (expensive) FAST_TASK(read_AHRS), #if FRAME_CONFIG == HELI_FRAME @@ -259,6 +260,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_BUTTON_ENABLED SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168), #endif +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + SCHED_TASK(update_dynamic_notch_at_specified_rate_main, LOOP_RATE, 200, 215), +#endif }; void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, @@ -618,12 +622,15 @@ void Copter::update_batt_compass(void) // should be run at loop rate void Copter::loop_rate_logging() { - if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) { + if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) { Log_Write_Attitude(); - Log_Write_PIDS(); // only logs if PIDS bitmask is set + if (!using_rate_thread) { + Log_Write_Rate(); + Log_Write_PIDS(); // only logs if PIDS bitmask is set + } } #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED - if (should_log(MASK_LOG_FTN_FAST)) { + if (should_log(MASK_LOG_FTN_FAST) && !using_rate_thread) { AP::ins().write_notch_log_messages(); } #endif @@ -641,10 +648,15 @@ void Copter::ten_hz_logging_loop() // log attitude controller data if we're not already logging at the higher rate if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) { Log_Write_Attitude(); + if (!using_rate_thread) { + Log_Write_Rate(); + } } if (!should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) { // log at 10Hz if PIDS bitmask is selected, even if no ATT bitmask is selected; logs at looprate if ATT_FAST and PIDS bitmask set - Log_Write_PIDS(); + if (!using_rate_thread) { + Log_Write_PIDS(); + } } // log EKF attitude data always at 10Hz unless ATTITUDE_FAST, then do it in the 25Hz loop if (!should_log(MASK_LOG_ATTITUDE_FAST)) { @@ -789,11 +801,24 @@ void Copter::one_hz_loop() AP_Notify::flags.flying = !ap.land_complete; // slowly update the PID notches with the average loop rate - attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + if (!using_rate_thread) { + attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); + } pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); #if AC_CUSTOMCONTROL_MULTI_ENABLED custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); #endif + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + // see if we should have a separate rate thread + if (!started_rate_thread && get_fast_rate_type() != FastRateType::FAST_RATE_DISABLED) { + if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void), + "rate", + 1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { + started_rate_thread = true; + } + } +#endif } void Copter::init_simple_bearing() diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index db74c31dc4..0ccdeed0da 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -631,6 +631,17 @@ private: RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4 REQUIRE_POSITION_FOR_ARMING = (1<<3), // 8 }; + + // type of fast rate attitude controller in operation + enum class FastRateType : uint8_t { + FAST_RATE_DISABLED = 0, + FAST_RATE_DYNAMIC = 1, + FAST_RATE_FIXED_ARMED = 2, + FAST_RATE_FIXED = 3, + }; + + FastRateType get_fast_rate_type() const { return FastRateType(g2.att_enable.get()); } + // returns true if option is enabled for this vehicle bool option_is_enabled(FlightOption option) const { return (g2.flight_options & uint32_t(option)) != 0; @@ -728,7 +739,18 @@ private: void set_accel_throttle_I_from_pilot_throttle(); void rotate_body_frame_to_NE(float &x, float &y); uint16_t get_pilot_speed_dn() const; - void run_rate_controller(); + void run_rate_controller_main(); + + // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + uint8_t calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz); + void rate_controller_thread(); + void rate_controller_filter_update(); + void rate_controller_log_update(); + uint8_t rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high); + void enable_fast_rate_loop(uint8_t rate_decimation); + void disable_fast_rate_loop(); + void update_dynamic_notch_at_specified_rate_main(); + // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED #if AC_CUSTOMCONTROL_MULTI_ENABLED void run_custom_controller() { custom_control.update(); } @@ -878,6 +900,7 @@ private: // Log.cpp void Log_Write_Control_Tuning(); void Log_Write_Attitude(); + void Log_Write_Rate(); void Log_Write_EKF_POS(); void Log_Write_PIDS(); void Log_Write_Data(LogDataID id, int32_t value); @@ -921,7 +944,8 @@ private: // motors.cpp void arm_motors_check(); void auto_disarm_check(); - void motors_output(); + void motors_output(bool full_push = true); + void motors_output_main(); void lost_vehicle_check(); // navigation.cpp @@ -1086,6 +1110,9 @@ private: Mode *mode_from_mode_num(const Mode::Number mode); void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); + bool started_rate_thread; + bool using_rate_thread; + public: void failsafe_check(); // failsafe.cpp }; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index fffea298f1..b446904e1a 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -76,6 +76,10 @@ void Copter::Log_Write_Control_Tuning() void Copter::Log_Write_Attitude() { attitude_control->Write_ANG(); +} + +void Copter::Log_Write_Rate() +{ attitude_control->Write_Rate(*pos_control); } diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index fd5eea3e86..37309440fb 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1,6 +1,7 @@ #include "Copter.h" #include +#include /* This program is free software: you can redistribute it and/or modify @@ -1232,6 +1233,22 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { // @User: Advanced AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT), +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + // @Param: FSTRATE_ENABLE + // @DisplayName: Enable the fast Rate thread + // @Description: Enable the fast Rate thread + // @User: Advanced + // @Values: 0:Disabled,1:Enabled-Dynamic,2:Enabled-FixedWhenArmed,3:Enabled-Fixed + AP_GROUPINFO("FSTRATE_ENABLE", 9, ParametersG2, att_enable, 0), + + // @Param: FSTRATE_DIV + // @DisplayName: Fast rate thread divisor + // @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate divided by this value. + // @User: Advanced + // @Range: 1 10 + AP_GROUPINFO("FSTRATE_DIV", 10, ParametersG2, att_decimation, 1), +#endif + // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0cf0a8cb51..2a85bc9ee9 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -690,6 +690,9 @@ public: AP_Float pldp_range_finder_maximum_m; AP_Float pldp_delay_s; AP_Float pldp_descent_speed_ms; + + AP_Int8 att_enable; + AP_Int8 att_decimation; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index db1e0ddc26..1462454b78 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -414,6 +414,7 @@ void ModeSystemId::log_data() const // Full rate logging of attitude, rate and pid loops copter.Log_Write_Attitude(); + copter.Log_Write_Rate(); copter.Log_Write_PIDS(); if (is_poscontrol_axis_type()) { diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 2313c30953..b40e5bc4ae 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -158,6 +158,12 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t ap.motor_test = true; EXPECT_DELAY_MS(3000); + + // wait for rate thread to stop running due to motor test + while (using_rate_thread) { + hal.scheduler->delay(1); + } + // enable and arm motors if (!motors->armed()) { motors->output_min(); // output lowest possible value to motors diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index c37a4d55a2..784580b8ec 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -133,7 +133,7 @@ void Copter::auto_disarm_check() } // motors_output - send output to motors library which will adjust and send to ESCs and servos -void Copter::motors_output() +void Copter::motors_output(bool full_push) { #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // this is to allow the failsafe module to deliberately crash @@ -183,7 +183,19 @@ void Copter::motors_output() } // push all channels - srv.push(); + if (full_push) { + srv.push(); + } else { + hal.rcout->push(); + } +} + +// motors_output from main thread +void Copter::motors_output_main() +{ + if (!using_rate_thread) { + motors_output(); + } } // check for pilot stick input to trigger lost vehicle alarm diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp new file mode 100644 index 0000000000..29434215aa --- /dev/null +++ b/ArduCopter/rate_thread.cpp @@ -0,0 +1,486 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#include "Copter.h" +#include +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + +#pragma GCC optimize("O2") + +/* + Attitude Rate controller thread design. + + Rationale: running rate outputs linked to fast gyro outputs achieves two goals: + + 1. High frequency gyro processing allows filters to be applied with high sample rates + which is advantageous in removing high frequency noise and associated aliasing + 2. High frequency rate control reduces the latency between control and action leading to + better disturbance rejection and faster responses which generally means higher + PIDs can be used without introducing control oscillation + + (1) is already mostly achieved through the higher gyro rates that are available via + INS_GYRO_RATE. (2) requires running the rate controller at higher rates via a separate thread + + + Goal: the ideal scenario is to run in a single cycle: + + gyro read->filter->publish->rate control->motor output + + This ensures the minimum latency between gyro sample and motor output. Other functions need + to also run faster than they would normally most notably logging and filter frequencies - most + notably the harmonic notch frequency. + + Design assumptions: + + 1. The sample rate of the IMUs is consistent and accurate. + This is the most basic underlying assumption. An alternative approach would be to rely on + the timing of when samples are received but this proves to not work in practice due to + scheduling delays. Thus the dt used by the attitude controller is the delta between IMU + measurements, not the delta between processing cycles in the rate thread. + 2. Every IMU reading must be processed or consistently sub-sampled. + This is an assumption that follows from (1) - so it means that attitude control should + process every sample or every other sample or every third sample etc. Note that these are + filtered samples - all incoming samples are processed for filtering purposes, it is only + for the purposes of rate control that we are sub-sampling. + 3. The data that the rate loop requires is timely, consistent and accurate. + Rate control essentially requires two components - the target and the actuals. The actuals + come from the incoming gyro sample combined with the state of the PIDs. The target comes + from attitude controller which is running at a slower rate in the main loop. Since the rate + thread can read the attitude target at any time it is important that this is always available + consistently and is updated consistently. + 4. The data that the rest of the vehicle uses is the same data that the rate thread uses. + Put another way any gyro value that the vehicle uses (e.g. in the EKF etc), must have already + been processed by the rate thread. Where this becomes important is with sub-sampling - if + rate gyro values are sub-sampled we need to make sure that the vehicle is also only using + the sub-sampled values. + + Design: + + 1. Filtered gyro samples are (sub-sampled and) pushed into an ObjectBuffer from the INS backend. + 2. The pushed sample is published to the INS front-end so that the rest of the vehicle only + sees published values that have been used by the rate controller. When the rate thread is not + in use the filtered samples are effectively sub-sampled at the main loop rate. The EKF is unaffected + as it uses delta angles calculated from the raw gyro values. (It might be possible to avoid publishing + from the rate thread by only updating _gyro_filtered when a value is pushed). + 3. A notification is sent that a sample is available + 4. The rate thread is blocked waiting for a sample. When it receives a notification it: + 4a. Runs the rate controller + 4b. Pushes the new pwm values. Periodically at the main loop rate all of the SRV_Channels::push() + functionality is run as well. + 5. The rcout dshot thread is blocked waiting for a new pwm value. When it is signalled by the + rate thread it wakes up and runs the dshot motor output logic. + 6. Periodically the rate thread: + 6a. Logs the rate outputs (1Khz) + 6b. Updates the notch filter centers (Gyro rate/2) + 6c. Checks the ObjectBuffer length and main loop delay (10Hz) + If the ObjectBuffer length has been longer than 2 for the last 5 cycles or the main loop has + been slowed down then the rate thread is slowed down by telling the INS to sub-sample. This + mechanism is continued until the rate thread is able to keep up with the sub-sample rate. + The inverse of this mechanism is run if the rate thread is able to keep up but is running slower + than the gyro sample rate. + 6d. Updates the PID notch centers (1Hz) + 7. When the rate rate changes through sub-sampling the following values are updated: + 7a. The PID notch sample rate + 7b. The dshot rate is constrained to be never greater than the gyro rate or rate rate + 7c. The motors dt + 8. Independently of the rate thread the attitude control target is updated in the main loop. In order + for target values to be consistent all updates are processed using local variables and the final + target is only written at the end of the update as a vector. Direct control of the target (e.g. in + autotune) is also constrained to be on all axes simultaneously using the new desired value. The + target makes use of the current PIDs and the "latest" gyro, it might be possible to use a loop + delayed gyro value, but that is currently out-of-scope. + + Performance considerations: + + On an H754 using ICM42688 and gyro sampling at 4KHz and rate thread at 4Khz the main CPU users are: + + ArduCopter PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=18.6% + idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD= 4.3% + rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD=10.7% + SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=17.5% + SPI4 PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=18.3% + rate PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=22.4% + + There is a direct correlation between the rate rate and CPU load, so if the rate rate is half the gyro + rate (i.e. 2Khz) we observe the following: + + ArduCopter PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=16.7% + idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD=21.3% + rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 6.2% + SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=16.7% + SPI4 PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=17.8% + rate PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=11.5% + + So we get almost a halving of CPU load in the rate and rcout threads. This is the main way that CPU + load can be handled on lower-performance boards, with the other mechanism being lowering the gyro rate. + So at a very respectable gyro rate and rate rate both of 2Khz (still 5x standard main loop rate) we see: + + ArduCopter PRI=182 sp=0x30000600 STACK=4440/7168 LOAD=15.6% + idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD=39.4% + rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 5.9% + SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD= 8.9% + SPI4 PRI=181 sp=0x3002D4A0 STACK= 896/1464 LOAD= 9.1% + rate PRI=182 sp=0x30029FB0 STACK=1296/1976 LOAD=11.8% + + This essentially means that its possible to run this scheme successfully on all MCUs by careful setting of + the maximum rates. + + Enabling rate thread timing debug for 4Khz reads with fast logging and armed we get the following data: + + Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us + Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us + Rate loop timing: gyro=177us, rate=13us, motors=46us, log=7us, ctrl=1us + + The log output is an average since it only runs at 1Khz, so roughly 28us elapsed. So the majority of the time + is spent waiting for a gyro sample (higher is better here since it represents the idle time) updating the PIDs + and outputting to the motors. Everything else is relatively cheap. Since the total cycle time is 250us the duty + cycle is thus 29% + */ + +#define DIV_ROUND_INT(x, d) ((x + d/2) / d) + +uint8_t Copter::calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz) +{ + return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)); +} + +//#define RATE_LOOP_TIMING_DEBUG +/* + thread for rate control +*/ +void Copter::rate_controller_thread() +{ + uint8_t target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400)); + uint8_t rate_decimation = target_rate_decimation; + uint32_t rate_loop_count = 0; + uint32_t prev_loop_count = 0; + + uint32_t last_run_us = AP_HAL::micros(); + float max_dt = 0.0; + float min_dt = 1.0; + uint32_t now_ms = AP_HAL::millis(); + uint32_t last_rate_check_ms = 0; + uint32_t last_rate_increase_ms = 0; +#if HAL_LOGGING_ENABLED + uint32_t last_rtdt_log_ms = now_ms; +#endif + uint32_t last_notch_sample_ms = now_ms; + bool was_using_rate_thread = false; + bool notify_fixed_rate_active = true; + bool was_armed = false; + uint32_t running_slow = 0; +#ifdef RATE_LOOP_TIMING_DEBUG + uint32_t gyro_sample_time_us = 0; + uint32_t rate_controller_time_us = 0; + uint32_t motor_output_us = 0; + uint32_t log_output_us = 0; + uint32_t ctrl_output_us = 0; + uint32_t timing_count = 0; + uint32_t last_timing_msg_us = 0; +#endif + + // run the filters at half the gyro rate + uint8_t filter_rate_decimate = 2; + uint8_t log_fast_rate_decimate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz +#if HAL_LOGGING_ENABLED + uint8_t log_med_rate_decimate = calc_gyro_decimation(rate_decimation, 10); // 10Hz + uint8_t log_loop_count = 0; +#endif + uint8_t main_loop_rate_decimate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); + uint8_t main_loop_count = 0; + uint8_t filter_loop_count = 0; + + while (true) { + +#ifdef RATE_LOOP_TIMING_DEBUG + uint32_t rate_now_us = AP_HAL::micros(); +#endif + + // allow changing option at runtime + if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) { + if (was_using_rate_thread) { + disable_fast_rate_loop(); + was_using_rate_thread = false; + } + hal.scheduler->delay_microseconds(500); + last_run_us = AP_HAL::micros(); + continue; + } + + // set up rate thread requirements + if (!using_rate_thread) { + enable_fast_rate_loop(rate_decimation); + } + ins.set_rate_decimation(rate_decimation); + + // wait for an IMU sample + Vector3f gyro; + if (!ins.get_next_gyro_sample(gyro)) { + continue; // go around again + } + +#ifdef RATE_LOOP_TIMING_DEBUG + gyro_sample_time_us += AP_HAL::micros() - rate_now_us; + rate_now_us = AP_HAL::micros(); +#endif + + // we must use multiples of the actual sensor rate + const float sensor_dt = 1.0f * rate_decimation / ins.get_raw_gyro_rate_hz(); + const uint32_t now_us = AP_HAL::micros(); + const uint32_t dt_us = now_us - last_run_us; + const float dt = dt_us * 1.0e-6; + last_run_us = now_us; + + max_dt = MAX(dt, max_dt); + min_dt = MIN(dt, min_dt); + +#if HAL_LOGGING_ENABLED +// @LoggerMessage: RTDT +// @Description: Attitude controller time deltas +// @Field: TimeUS: Time since system startup +// @Field: dt: current time delta +// @Field: dtAvg: current time delta average +// @Field: dtMax: Max time delta since last log output +// @Field: dtMin: Min time delta since last log output + + if (now_ms - last_rtdt_log_ms >= 10) { // 100 Hz + AP::logger().WriteStreaming("RTDT", "TimeUS,dt,dtAvg,dtMax,dtMin", "Qffff", + AP_HAL::micros64(), + dt, sensor_dt, max_dt, min_dt); + max_dt = sensor_dt; + min_dt = sensor_dt; + last_rtdt_log_ms = now_ms; + } +#endif + + motors->set_dt(sensor_dt); + // check if we are falling behind + if (ins.get_num_gyro_samples() > 2) { + running_slow++; + } else if (running_slow > 0) { + running_slow--; + } + if (AP::scheduler().get_extra_loop_us() == 0) { + rate_loop_count++; + } + + // run the rate controller on all available samples + // it is important not to drop samples otherwise the filtering will be fubar + // there is no need to output to the motors more than once for every batch of samples + attitude_control->rate_controller_run_dt(sensor_dt, gyro + ahrs.get_gyro_drift()); + +#ifdef RATE_LOOP_TIMING_DEBUG + rate_controller_time_us += AP_HAL::micros() - rate_now_us; + rate_now_us = AP_HAL::micros(); +#endif + + // immediately output the new motor values + if (main_loop_count++ >= main_loop_rate_decimate) { + main_loop_count = 0; + } + motors_output(main_loop_count == 0); + + // process filter updates + if (filter_loop_count++ >= filter_rate_decimate) { + filter_loop_count = 0; + rate_controller_filter_update(); + } + +#ifdef RATE_LOOP_TIMING_DEBUG + motor_output_us += AP_HAL::micros() - rate_now_us; + rate_now_us = AP_HAL::micros(); +#endif + +#if HAL_LOGGING_ENABLED + // fast logging output + if (should_log(MASK_LOG_ATTITUDE_FAST)) { + if (log_fast_rate_decimate > 0 && log_loop_count++ >= log_fast_rate_decimate) { + log_loop_count = 0; + rate_controller_log_update(); + } + } else if (should_log(MASK_LOG_ATTITUDE_MED)) { + if (log_med_rate_decimate > 0 && log_loop_count++ >= log_med_rate_decimate) { + log_loop_count = 0; + rate_controller_log_update(); + } + } +#else + (void)log_fast_rate_decimate; +#endif + +#ifdef RATE_LOOP_TIMING_DEBUG + log_output_us += AP_HAL::micros() - rate_now_us; + rate_now_us = AP_HAL::micros(); +#endif + + now_ms = AP_HAL::millis(); + + // make sure we have the latest target rate + target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400)); + if (now_ms - last_notch_sample_ms >= 1000 || !was_using_rate_thread) { + // update the PID notch sample rate at 1Hz if we are + // enabled at runtime + last_notch_sample_ms = now_ms; + attitude_control->set_notch_sample_rate(1.0 / sensor_dt); + } + + // interlock for printing fixed rate active + if (was_armed != motors->armed()) { + notify_fixed_rate_active = !was_armed; + was_armed = motors->armed(); + } + + // Once armed, switch to the fast rate if configured to do so + if ((rate_decimation != target_rate_decimation || notify_fixed_rate_active) + && ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed()) + || get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) { + rate_decimation = target_rate_decimation; + log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false); + notify_fixed_rate_active = false; + } + + // check that the CPU is not pegged, if it is drop the attitude rate + if (now_ms - last_rate_check_ms >= 100 + && (get_fast_rate_type() == FastRateType::FAST_RATE_DYNAMIC + || (get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && !motors->armed()) + || target_rate_decimation > rate_decimation)) { + last_rate_check_ms = now_ms; + const uint32_t att_rate = ins.get_raw_gyro_rate_hz()/rate_decimation; + if (running_slow > 5 || AP::scheduler().get_extra_loop_us() > 0 +#if HAL_LOGGING_ENABLED + || AP::logger().in_log_download() +#endif + || target_rate_decimation > rate_decimation) { + const uint8_t new_rate_decimation = MAX(rate_decimation + 1, target_rate_decimation); + const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz() / new_rate_decimation; + if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz()) { + rate_decimation = new_rate_decimation; + log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, true); + prev_loop_count = rate_loop_count; + rate_loop_count = 0; + running_slow = 0; + } + } else if (rate_decimation > target_rate_decimation && rate_loop_count > att_rate/10 // ensure 100ms worth of good readings + && (prev_loop_count > att_rate/10 // ensure there was 100ms worth of good readings at the higher rate + || prev_loop_count == 0 // last rate was actually a lower rate so keep going quickly + || now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry + rate_decimation = rate_decimation - 1; + + log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false); + prev_loop_count = 0; + rate_loop_count = 0; + last_rate_increase_ms = now_ms; + } + } + +#ifdef RATE_LOOP_TIMING_DEBUG + timing_count++; + ctrl_output_us += AP_HAL::micros() - rate_now_us; + rate_now_us = AP_HAL::micros(); + + if (rate_now_us - last_timing_msg_us > 1e6) { + hal.console->printf("Rate loop timing: gyro=%uus, rate=%uus, motors=%uus, log=%uus, ctrl=%uus\n", + unsigned(gyro_sample_time_us/timing_count), unsigned(rate_controller_time_us/timing_count), + unsigned(motor_output_us/timing_count), unsigned(log_output_us/timing_count), unsigned(ctrl_output_us/timing_count)); + last_timing_msg_us = rate_now_us; + timing_count = 0; + gyro_sample_time_us = rate_controller_time_us = motor_output_us = log_output_us = ctrl_output_us = 0; + } +#endif + + was_using_rate_thread = true; + } +} + +/* + update rate controller filters. on an H7 this is about 30us +*/ +void Copter::rate_controller_filter_update() +{ + // update the frontend center frequencies of notch filters + for (auto ¬ch : ins.harmonic_notches) { + update_dynamic_notch(notch); + } + + // this copies backend data to the frontend and updates the notches + ins.update_backend_filters(); +} + +/* + update rate controller rates and return the logging rate +*/ +uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high) +{ + const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation; + attitude_control->set_notch_sample_rate(attitude_rate); + hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), attitude_rate); + motors->set_dt(1.0f / attitude_rate); + gcs().send_text(warn_cpu_high ? MAV_SEVERITY_WARNING : MAV_SEVERITY_INFO, + "Rate CPU %s, rate set to %uHz", + warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate); +#if HAL_LOGGING_ENABLED + if (attitude_rate > 1000) { + return calc_gyro_decimation(rate_decimation, 1000); + } else { + return calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); + } +#endif + return 0; +} + +void Copter::enable_fast_rate_loop(uint8_t rate_decimation) +{ + ins.enable_fast_rate_buffer(); + rate_controller_set_rates(rate_decimation, false); + hal.rcout->force_trigger_groups(true); + using_rate_thread = true; +} + +void Copter::disable_fast_rate_loop() +{ + using_rate_thread = false; + rate_controller_set_rates(calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz()), false); + hal.rcout->force_trigger_groups(false); + ins.disable_fast_rate_buffer(); +} + +/* + log only those items that are updated at the rate loop rate + */ +void Copter::rate_controller_log_update() +{ +#if HAL_LOGGING_ENABLED + if (!copter.flightmode->logs_attitude()) { + Log_Write_Rate(); + Log_Write_PIDS(); // only logs if PIDS bitmask is set + } +#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED + if (should_log(MASK_LOG_FTN_FAST)) { + AP::ins().write_notch_log_messages(); + } +#endif +#endif +} + +// run notch update at either loop rate or 200Hz +void Copter::update_dynamic_notch_at_specified_rate_main() +{ + if (using_rate_thread) { + return; + } + + update_dynamic_notch_at_specified_rate(); +} + +#endif // AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED From 31dc7a85bfbe637f717c311e35a4088ac40ac401 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 22 May 2024 21:35:30 +0100 Subject: [PATCH 26/47] autotest: add rate thread autotest --- Tools/autotest/arducopter.py | 72 ++++++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f1dccfcf1b..8b46a62473 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6293,6 +6293,77 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): if ex is not None: raise ex + def DynamicRpmNotchesRateThread(self): + """Use dynamic harmonic notch to control motor noise via ESC telemetry.""" + self.progress("Flying with ESC telemetry driven dynamic notches") + self.context_push() + self.set_rc_default() + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour + "LOG_BITMASK": 959, + "LOG_DISARMED": 0, + "SIM_VIB_MOT_MAX": 350, + "SIM_GYR1_RND": 20, + "SIM_ESC_TELEM": 1, + "FSTRATE_ENABLE": 1 + }) + self.reboot_sitl() + + self.takeoff(10, mode="ALT_HOLD") + + # find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe. + # there is a second harmonic at 380Hz which should be avoided to make the test reliable + # detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB + freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320) + + # now add a dynamic notch and check that the peak is squashed + self.set_parameters({ + "INS_LOG_BAT_OPT": 4, + "INS_HNTCH_ENABLE": 1, + "INS_HNTCH_FREQ": 80, + "INS_HNTCH_REF": 1.0, + "INS_HNTCH_HMNCS": 5, # first and third harmonic + "INS_HNTCH_ATT": 50, + "INS_HNTCH_BW": 40, + "INS_HNTCH_MODE": 3, + "FSTRATE_ENABLE": 1 + }) + self.reboot_sitl() + + # -10dB is pretty conservative - actual is about -25dB + freq, hover_throttle, peakdb1, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + esc_hz = self.get_average_esc_frequency() + esc_peakdb1 = psd["X"][int(esc_hz)] + + # now add notch-per motor and check that the peak is squashed + self.set_parameter("INS_HNTCH_OPTS", 2) + self.reboot_sitl() + + freq, hover_throttle, peakdb2, psd = \ + self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2) + # find the noise at the motor frequency + esc_hz = self.get_average_esc_frequency() + esc_peakdb2 = psd["X"][int(esc_hz)] + + # notch-per-motor will be better at the average ESC frequency + if esc_peakdb2 > esc_peakdb1: + raise NotAchievedException( + "Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" % + (esc_peakdb2, esc_peakdb1)) + + # check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily + # testing shows this to be -58dB on average + if esc_peakdb2 > -25: + raise NotAchievedException( + "Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2) + self.context_pop() + self.reboot_sitl() + def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None): '''do a simple up-and-down test flight with current vehicle state. Check that the onboard filter comes up with the same peak-frequency that @@ -12160,6 +12231,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): Test(self.DynamicNotches, attempts=4), self.PositionWhenGPSIsZero, self.DynamicRpmNotches, # Do not add attempts to this - failure is sign of a bug + self.DynamicRpmNotchesRateThread, self.PIDNotches, self.StaticNotches, self.RefindGPS, From 6ac35ce9afb8ce89af4164f6b72d2109541c2e70 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Feb 2024 12:06:22 +1100 Subject: [PATCH 27/47] AP_Vehicle: use update_dynamic_notch() directly in rate loop --- libraries/AP_Vehicle/AP_Vehicle.cpp | 3 ++- libraries/AP_Vehicle/AP_Vehicle.h | 15 ++++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b79373b364..9f7d70bb24 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -3,6 +3,7 @@ #if AP_VEHICLE_ENABLED #include "AP_Vehicle.h" +#include #include #include @@ -621,7 +622,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210), #endif -#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED +#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && !AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED SCHED_TASK(update_dynamic_notch_at_specified_rate, LOOP_RATE, 200, 215), #endif #if AP_VIDEOTX_ENABLED diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index cba506cb9b..f34be5accb 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -522,6 +522,13 @@ protected: // Check if this mode can be entered from the GCS bool block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const; +#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED + // update the harmonic notch + void update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch); + // run notch update at either loop rate or 200Hz + void update_dynamic_notch_at_specified_rate(); +#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED + private: #if AP_SCHEDULER_ENABLED @@ -536,13 +543,7 @@ private: #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED // update the harmonic notch for throttle based notch void update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch); - - // update the harmonic notch - void update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch); - - // run notch update at either loop rate or 200Hz - void update_dynamic_notch_at_specified_rate(); -#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED +#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED // decimation for 1Hz update uint8_t one_Hz_counter; From c4ab8e25c4ee7ab4720d6abfb30d5e4aacf4a434 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 7 Jun 2024 10:30:02 +0100 Subject: [PATCH 28/47] AP_HAL: allow forcing of trigger_groups() --- libraries/AP_HAL/RCOutput.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index 68a84b1a10..dee9068138 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -376,6 +376,11 @@ public: */ virtual void write_gpio(uint8_t chan, bool active) {}; + /* + Force group trigger from all callers rather than just from the main thread + */ + virtual void force_trigger_groups(bool onoff) {}; + /* * calculate the prescaler required to achieve the desire bitrate */ From 76897e9674364cad0f789542babf810a6607d010 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 20:00:44 +0100 Subject: [PATCH 29/47] AP_HAL: set HAL_INS_RATE_LOOP in boards restrict rate loop to H7 and F7 --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ libraries/AP_HAL/board/chibios.h | 6 ++++++ libraries/AP_HAL/board/linux.h | 4 ++++ libraries/AP_HAL/board/sitl.h | 4 ++++ 4 files changed, 18 insertions(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 26a47e2c8c..b5dddb89cb 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -385,4 +385,8 @@ #define HAL_WITH_POSTYPE_DOUBLE BOARD_FLASH_SIZE > 1024 #endif +#ifndef HAL_INS_RATE_LOOP +#define HAL_INS_RATE_LOOP 0 +#endif + #define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON) diff --git a/libraries/AP_HAL/board/chibios.h b/libraries/AP_HAL/board/chibios.h index 9e5cd5911f..9c51ac58ea 100644 --- a/libraries/AP_HAL/board/chibios.h +++ b/libraries/AP_HAL/board/chibios.h @@ -140,3 +140,9 @@ #endif #define HAL_USE_QUADSPI (HAL_USE_QUADSPI1 || HAL_USE_QUADSPI2) #define HAL_USE_OCTOSPI (HAL_USE_OCTOSPI1 || HAL_USE_OCTOSPI2) + +#if defined(STM32H7) || defined(STM32F7) +#define HAL_INS_RATE_LOOP 1 +#else +#define HAL_INS_RATE_LOOP 0 +#endif diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index a7e6c3fc17..c55fec67e2 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -422,3 +422,7 @@ #else #define HAL_LINUX_USE_VIRTUAL_CAN 0 #endif + +#ifndef HAL_INS_RATE_LOOP +#define HAL_INS_RATE_LOOP 1 +#endif diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index 7738814898..e4d1afff91 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -96,3 +96,7 @@ #endif #define HAL_SOLO_GIMBAL_ENABLED 1 + +#ifndef HAL_INS_RATE_LOOP +#define HAL_INS_RATE_LOOP 1 +#endif From 048a3af785e7589fb4e2ffef52326e54d90b2e3d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 7 Jun 2024 16:36:08 +0100 Subject: [PATCH 30/47] AP_HAL_ChibiOS: allow forcing of trigger_groups() --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 3 ++- libraries/AP_HAL_ChibiOS/RCOutput.h | 7 +++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 3d3245444f..1e0fbae0a3 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1398,7 +1398,8 @@ void RCOutput::trigger_groups() osalSysUnlock(); #if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED // trigger a PWM send - if (!in_soft_serial() && hal.scheduler->in_main_thread() && rcout_thread_ctx) { + if (!in_soft_serial() && + (hal.scheduler->in_main_thread() || force_trigger) && rcout_thread_ctx) { chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND); } #endif diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 87aa9948db..a935de7f45 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -285,6 +285,11 @@ public: */ void rcout_thread(); + /* + Force group trigger from all callers rather than just from the main thread + */ + void force_trigger_groups(bool onoff) override { force_trigger = onoff; } + /* timer information */ @@ -579,6 +584,8 @@ private: uint8_t _dshot_cycle; // virtual timer for post-push() pulses virtual_timer_t _dshot_rate_timer; + // force triggering of groups + bool force_trigger; #if HAL_DSHOT_ENABLED // dshot commands From 3cc4b1991c139d18212bca6c6d98e706b4cefe9a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 3 Jul 2024 18:26:11 +0100 Subject: [PATCH 31/47] AP_HAL_ChibiOS: compile MambaH743v4 at -O2 --- libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat index 661909c0d7..f3179a3cb4 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat @@ -20,6 +20,7 @@ define CH_CFG_ST_RESOLUTION 16 # leave 1 sectors free FLASH_RESERVE_START_KB 384 +env OPTIMIZE -O2 # use last 2 pages for flash storage # H743 has 16 pages of 128k each From a3de217deec3eb18ed971c68cdba4ace0350417c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 11 Jul 2024 20:51:31 +0100 Subject: [PATCH 32/47] AP_HAL_ChibiOS: ensure dshot rate can be set dynamically honour the requested dshot rate as near as possible --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1e0fbae0a3..33fbe4ac69 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -553,19 +553,19 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) } uint16_t drate = dshot_rate * loop_rate_hz; - _dshot_rate = dshot_rate; // BLHeli32 uses a 16 bit counter for input calibration which at 48Mhz will wrap // at 732Hz so never allow rates below 800hz while (drate < 800) { - _dshot_rate++; - drate = _dshot_rate * loop_rate_hz; + dshot_rate++; + drate = dshot_rate * loop_rate_hz; } - // prevent stupidly high rates, ideally should also prevent high rates + // prevent stupidly high rate multiples, ideally should also prevent high rates // with slower dshot variants - if (drate > 4000) { - _dshot_rate = 4000 / loop_rate_hz; - drate = _dshot_rate * loop_rate_hz; + while (dshot_rate > 1 && drate > MAX(4096, loop_rate_hz)) { + dshot_rate--; + drate = dshot_rate * loop_rate_hz; } + _dshot_rate = dshot_rate; _dshot_period_us = 1000000UL / drate; #if HAL_WITH_IO_MCU if (iomcu_dshot) { From ba6da82f07b4945ae7da0514b3c5c459cb8eabe4 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 22 May 2024 12:34:29 +0100 Subject: [PATCH 33/47] scripts: add AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED to build_options.py and extract_features.py --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 902f77fb2a..e1e7d5433a 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -343,6 +343,7 @@ BUILD_OPTIONS = [ Feature('IMU', 'HarmonicNotches', 'AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', 'Enable InertialSensor harmonic notch filters', 0, None), # noqa Feature('IMU', 'BatchSampler', 'AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', 'Enable Batch sampler', 0, None), # noqa + Feature('Other', 'RateLoopThread', 'AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED', 'Enable Rate Loop Thread', 0, None), # noqa Feature('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight gyro FFT calculations', 0, None), Feature('Other', 'NMEA_OUTPUT', 'HAL_NMEA_OUTPUT_ENABLED', 'Enable NMEA output', 0, None), Feature('Other', 'SDCARD_FORMATTING', 'AP_FILESYSTEM_FORMAT_ENABLED', 'Enable Formatting of microSD cards', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 6e51e333e8..1a145dcc22 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -210,6 +210,7 @@ class ExtractFeatures(object): ('HAL_WITH_DSP', r'AP_HAL::DSP::find_peaks\b',), ('AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', r'AP_InertialSensor::HarmonicNotch::update_params\b',), ('AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', r'AP_InertialSensor::BatchSampler::init'), + ('AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED', r'FastRateBuffer::get_next_gyro_sample\b',), ('HAL_GYROFFT_ENABLED', r'AP_GyroFFT::AP_GyroFFT\b',), ('HAL_DISPLAY_ENABLED', r'Display::init\b',), ('HAL_NMEA_OUTPUT_ENABLED', r'AP_NMEA_Output::update\b',), From 21106380b3eb671863e9ed85e2a001dedc3f710d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 7 Aug 2024 16:56:16 +0100 Subject: [PATCH 34/47] waf: add rate loop config abstraction that allows code to be elided on non-copter builds --- Tools/ardupilotwaf/ap_library.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index 9d0971c1e8..fbd355db39 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -54,7 +54,8 @@ def _vehicle_index(vehicle): # note that AP_NavEKF3_core.h is needed for AP_NavEKF3_feature.h _vehicle_macros = ['APM_BUILD_DIRECTORY', 'AP_BUILD_TARGET_NAME', 'APM_BUILD_TYPE', 'APM_BUILD_COPTER_OR_HELI', - 'AP_NavEKF3_core.h', 'lua_generated_bindings.h'] + 'AP_NavEKF3_core.h', 'lua_generated_bindings.h', + 'AP_InertialSensor_rate_config.h'] _macros_re = re.compile(r'\b(%s)\b' % '|'.join(_vehicle_macros)) # some cpp files are not available at the time we run this check so need to be @@ -174,6 +175,7 @@ class ap_library_check_headers(Task.Task): 'libraries/AP_Scripting/lua_generated_bindings.h', 'libraries/AP_NavEKF3/AP_NavEKF3_feature.h', 'libraries/AP_LandingGear/AP_LandingGear_config.h', + 'libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h', ) whitelist = tuple(os.path.join(*p.split('/')) for p in whitelist) From dcf25200c66a3eca862f91dd9d9c91367020ed84 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 25 Aug 2024 16:20:29 +0100 Subject: [PATCH 35/47] Copter: correctly set fast rate thread rates --- ArduCopter/Copter.h | 15 ++++++--- ArduCopter/rate_thread.cpp | 63 +++++++++++++++++++++++--------------- 2 files changed, 50 insertions(+), 28 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0ccdeed0da..cbfbcef10e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -742,13 +742,20 @@ private: void run_rate_controller_main(); // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED - uint8_t calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz); + struct RateControllerRates { + uint8_t fast_logging_rate; + uint8_t medium_logging_rate; + uint8_t filter_rate; + uint8_t main_loop_rate; + }; + + uint8_t calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz); void rate_controller_thread(); void rate_controller_filter_update(); void rate_controller_log_update(); - uint8_t rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high); - void enable_fast_rate_loop(uint8_t rate_decimation); - void disable_fast_rate_loop(); + uint8_t rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high); + void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates); + void disable_fast_rate_loop(RateControllerRates& rates); void update_dynamic_notch_at_specified_rate_main(); // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index 29434215aa..115c589729 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -150,11 +150,16 @@ #define DIV_ROUND_INT(x, d) ((x + d/2) / d) -uint8_t Copter::calc_gyro_decimation(uint16_t gyro_decimation, uint16_t rate_hz) +uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz) { return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)); } +static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count) +{ + return decimation_rate > 0 && ++decimation_count >= decimation_rate; +} + //#define RATE_LOOP_TIMING_DEBUG /* thread for rate control @@ -163,6 +168,11 @@ void Copter::rate_controller_thread() { uint8_t target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400)); uint8_t rate_decimation = target_rate_decimation; + + // set up the decimation rates + RateControllerRates rates; + rate_controller_set_rates(rate_decimation, rates, false); + uint32_t rate_loop_count = 0; uint32_t prev_loop_count = 0; @@ -191,13 +201,9 @@ void Copter::rate_controller_thread() #endif // run the filters at half the gyro rate - uint8_t filter_rate_decimate = 2; - uint8_t log_fast_rate_decimate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz #if HAL_LOGGING_ENABLED - uint8_t log_med_rate_decimate = calc_gyro_decimation(rate_decimation, 10); // 10Hz uint8_t log_loop_count = 0; #endif - uint8_t main_loop_rate_decimate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); uint8_t main_loop_count = 0; uint8_t filter_loop_count = 0; @@ -210,7 +216,7 @@ void Copter::rate_controller_thread() // allow changing option at runtime if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) { if (was_using_rate_thread) { - disable_fast_rate_loop(); + disable_fast_rate_loop(rates); was_using_rate_thread = false; } hal.scheduler->delay_microseconds(500); @@ -220,7 +226,7 @@ void Copter::rate_controller_thread() // set up rate thread requirements if (!using_rate_thread) { - enable_fast_rate_loop(rate_decimation); + enable_fast_rate_loop(rate_decimation, rates); } ins.set_rate_decimation(rate_decimation); @@ -278,7 +284,7 @@ void Copter::rate_controller_thread() // run the rate controller on all available samples // it is important not to drop samples otherwise the filtering will be fubar // there is no need to output to the motors more than once for every batch of samples - attitude_control->rate_controller_run_dt(sensor_dt, gyro + ahrs.get_gyro_drift()); + attitude_control->rate_controller_run_dt(gyro + ahrs.get_gyro_drift(), sensor_dt); #ifdef RATE_LOOP_TIMING_DEBUG rate_controller_time_us += AP_HAL::micros() - rate_now_us; @@ -286,14 +292,15 @@ void Copter::rate_controller_thread() #endif // immediately output the new motor values - if (main_loop_count++ >= main_loop_rate_decimate) { + if (run_decimated_callback(rates.main_loop_rate, main_loop_count)) { main_loop_count = 0; } motors_output(main_loop_count == 0); // process filter updates - if (filter_loop_count++ >= filter_rate_decimate) { + if (run_decimated_callback(rates.filter_rate, filter_loop_count)) { filter_loop_count = 0; + rate_controller_filter_update(); } @@ -305,18 +312,17 @@ void Copter::rate_controller_thread() #if HAL_LOGGING_ENABLED // fast logging output if (should_log(MASK_LOG_ATTITUDE_FAST)) { - if (log_fast_rate_decimate > 0 && log_loop_count++ >= log_fast_rate_decimate) { + if (run_decimated_callback(rates.fast_logging_rate, log_loop_count)) { log_loop_count = 0; rate_controller_log_update(); + } } else if (should_log(MASK_LOG_ATTITUDE_MED)) { - if (log_med_rate_decimate > 0 && log_loop_count++ >= log_med_rate_decimate) { + if (run_decimated_callback(rates.medium_logging_rate, log_loop_count)) { log_loop_count = 0; rate_controller_log_update(); } } -#else - (void)log_fast_rate_decimate; #endif #ifdef RATE_LOOP_TIMING_DEBUG @@ -333,6 +339,10 @@ void Copter::rate_controller_thread() // enabled at runtime last_notch_sample_ms = now_ms; attitude_control->set_notch_sample_rate(1.0 / sensor_dt); +#ifdef RATE_LOOP_TIMING_DEBUG + hal.console->printf("Sample rate %.1f, main loop %u, fast rate %u, med rate %u\n", 1.0 / sensor_dt, + rates.main_loop_rate, rates.fast_logging_rate, rates.medium_logging_rate); +#endif } // interlock for printing fixed rate active @@ -346,7 +356,7 @@ void Copter::rate_controller_thread() && ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed()) || get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) { rate_decimation = target_rate_decimation; - log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false); + rate_controller_set_rates(rate_decimation, rates, false); notify_fixed_rate_active = false; } @@ -366,7 +376,7 @@ void Copter::rate_controller_thread() const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz() / new_rate_decimation; if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz()) { rate_decimation = new_rate_decimation; - log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, true); + rate_controller_set_rates(rate_decimation, rates, true); prev_loop_count = rate_loop_count; rate_loop_count = 0; running_slow = 0; @@ -377,7 +387,7 @@ void Copter::rate_controller_thread() || now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry rate_decimation = rate_decimation - 1; - log_fast_rate_decimate = rate_controller_set_rates(rate_decimation, false); + rate_controller_set_rates(rate_decimation, rates, false); prev_loop_count = 0; rate_loop_count = 0; last_rate_increase_ms = now_ms; @@ -420,7 +430,7 @@ void Copter::rate_controller_filter_update() /* update rate controller rates and return the logging rate */ -uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu_high) +uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high) { const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation; attitude_control->set_notch_sample_rate(attitude_rate); @@ -431,26 +441,31 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, bool warn_cpu warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate); #if HAL_LOGGING_ENABLED if (attitude_rate > 1000) { - return calc_gyro_decimation(rate_decimation, 1000); + rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz } else { - return calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); + rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); } + rates.medium_logging_rate = calc_gyro_decimation(rate_decimation, 10); // 10Hz #endif + rates.main_loop_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); + rates.filter_rate = calc_gyro_decimation(rate_decimation, ins.get_raw_gyro_rate_hz() / 2); + return 0; } -void Copter::enable_fast_rate_loop(uint8_t rate_decimation) +void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates) { ins.enable_fast_rate_buffer(); - rate_controller_set_rates(rate_decimation, false); + rate_controller_set_rates(rate_decimation, rates, false); hal.rcout->force_trigger_groups(true); using_rate_thread = true; } -void Copter::disable_fast_rate_loop() +void Copter::disable_fast_rate_loop(RateControllerRates& rates) { using_rate_thread = false; - rate_controller_set_rates(calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz()), false); + uint8_t rate_decimation = calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz()); + rate_controller_set_rates(rate_decimation, rates, false); hal.rcout->force_trigger_groups(false); ins.disable_fast_rate_buffer(); } From 6db09c9fdde24ce8b148eb98b9316e657ab9cf02 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 11 Feb 2024 16:19:56 +1100 Subject: [PATCH 36/47] AP_InertialSensor: keep a queue of gyro samples for use by the rate thread decimate the gyro window locally configure rate loop buffer based on AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED allow backends to be updated from rate thread output debug error if rate loop buffer overruns add support for updating filter parameters independently of propagating samples add rate loop config abstraction that allows code to be elided on non-copter builds must be using harmonic notch to use rate thread mediate fast rate loop buffer using mutex and binary semaphore ensure gyro samples are used when the rate loop buffer isn't Co-Authored-By: Andrew Tridgell --- .../AP_InertialSensor/AP_InertialSensor.h | 33 ++++- .../AP_InertialSensor_Backend.cpp | 14 ++ .../AP_InertialSensor_Backend.h | 9 ++ .../AP_InertialSensor_rate_config.h | 9 ++ .../AP_InertialSensor/FastRateBuffer.cpp | 124 ++++++++++++++++++ libraries/AP_InertialSensor/FastRateBuffer.h | 50 +++++++ 6 files changed, 237 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h create mode 100644 libraries/AP_InertialSensor/FastRateBuffer.cpp create mode 100644 libraries/AP_InertialSensor/FastRateBuffer.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 30e60bec6c..446a46f285 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -9,6 +9,8 @@ #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout +#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop + #include #include @@ -34,6 +36,7 @@ class AP_InertialSensor_Backend; class AuxiliaryBus; class AP_AHRS; +class FastRateBuffer; /* forward declare AP_Logger class. We can't include logger.h @@ -51,6 +54,7 @@ class AP_Logger; class AP_InertialSensor : AP_AccelCal_Client { friend class AP_InertialSensor_Backend; + friend class FastRateBuffer; public: AP_InertialSensor(); @@ -161,12 +165,12 @@ public: const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_first_usable_gyro]; } FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; } FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_first_usable_gyro, axis); } - uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); } - uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; } #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED bool has_fft_notch() const; #endif #endif + uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; } + uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); } bool set_gyro_window_size(uint16_t size); // get accel offsets in m/s/s const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset(i); } @@ -263,6 +267,7 @@ public: AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance); void detect_backends(void); + void update_backends(); // accel peak hold detector void set_accel_peak_hold(uint8_t instance, const Vector3f &accel); @@ -798,6 +803,30 @@ private: bool raw_logging_option_set(RAW_LOGGING_OPTION option) const { return (raw_logging_options.get() & int32_t(option)) != 0; } + // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + // Support for the fast rate thread in copter + FastRateBuffer* fast_rate_buffer; + +public: + // enable the fast rate buffer and start pushing samples to it + void enable_fast_rate_buffer(); + // disable the fast rate buffer and stop pushing samples to it + void disable_fast_rate_buffer(); + // get the next available gyro sample from the fast rate buffer + bool get_next_gyro_sample(Vector3f& gyro); + // get the number of available gyro samples in the fast rate buffer + uint32_t get_num_gyro_samples(); + // set the rate at which samples are collected, unused samples are dropped + void set_rate_decimation(uint8_t rdec); + // are gyro samples being sourced from the rate loop buffer + bool use_rate_loop_gyro_samples() const; + // push a new gyro sample into the fast rate buffer + bool push_next_gyro_sample(const Vector3f& gyro); + // run the filter parmeter update code. + void update_backend_filters(); + // are rate loop samples enabled for this instance? + bool is_rate_loop_gyro_enabled(uint8_t instance) const; + // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED }; namespace AP { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 944b20661a..30d06b02b1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -2,6 +2,7 @@ #include #include +#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" #include @@ -254,9 +255,21 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const notch.filter[instance].reset(); } #endif + gyro_filtered = _imu._gyro_filtered[instance]; + } + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + if (_imu.is_rate_loop_gyro_enabled(instance)) { + if (_imu.push_next_gyro_sample(gyro_filtered)) { + // if we used the value, record it for publication to the front-end + _imu._gyro_filtered[instance] = gyro_filtered; + } } else { _imu._gyro_filtered[instance] = gyro_filtered; } +#else + _imu._gyro_filtered[instance] = gyro_filtered; +#endif } void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, @@ -772,6 +785,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ if (has_been_killed(instance)) { return; } + if (_imu._new_gyro_data[instance]) { _publish_gyro(instance, _imu._gyro_filtered[instance]); #if HAL_GYROFFT_ENABLED diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 817cb2e947..7ab5b420d8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -53,6 +53,15 @@ public: */ virtual bool update() = 0; /* front end */ + // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + /* + * Update the filter parameters. Called by the frontend to propagate + * filter parameters to the frontend structure via the + * update_gyro_filters() and update_accel_filters() functions + */ + void update_filters() __RAMFUNC__; /* front end */ + // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + /* * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h new file mode 100644 index 0000000000..8bf45fecab --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h @@ -0,0 +1,9 @@ +#pragma once + +#include +#include +#include + +#ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED +#define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter)) +#endif diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp new file mode 100644 index 0000000000..6617915fec --- /dev/null +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -0,0 +1,124 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include +#include "AP_InertialSensor_rate_config.h" +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED +#include "FastRateBuffer.h" +#include + +extern const AP_HAL::HAL& hal; + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +// hal.console can be accessed from bus threads on ChibiOS +#define debug(fmt, args ...) do {hal.console->printf("IMU: " fmt "\n", ## args); } while(0) +#else +#define debug(fmt, args ...) do {printf("IMU: " fmt "\n", ## args); } while(0) +#endif + +void AP_InertialSensor::enable_fast_rate_buffer() +{ + fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); +} + +void AP_InertialSensor::disable_fast_rate_buffer() +{ + delete fast_rate_buffer; + fast_rate_buffer = nullptr; +} + +uint32_t AP_InertialSensor::get_num_gyro_samples() +{ + return fast_rate_buffer->get_num_gyro_samples(); +} + +void AP_InertialSensor::set_rate_decimation(uint8_t rdec) +{ + fast_rate_buffer->set_rate_decimation(rdec); +} + +// are gyro samples being sourced from the rate loop buffer +bool AP_InertialSensor::use_rate_loop_gyro_samples() const +{ + return fast_rate_buffer != nullptr; +} + +// whether or not to push the current gyro sample +bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const +{ + return use_rate_loop_gyro_samples() && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); +} + +bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) +{ + if (!use_rate_loop_gyro_samples()) { + return false; + } + + return fast_rate_buffer->get_next_gyro_sample(gyro); +} + +bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) +{ + if (!use_rate_loop_gyro_samples()) { + return false; + } + + if (_rate_loop_gyro_window.available() == 0) { + _notifier.wait_blocking(); + } + + WITH_SEMAPHORE(_mutex); + + return _rate_loop_gyro_window.pop(gyro); +} + +bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) +{ + if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) { + return false; + } + /* + tell the rate thread we have a new sample + */ + WITH_SEMAPHORE(fast_rate_buffer->_mutex); + + if (!fast_rate_buffer->_rate_loop_gyro_window.push(gyro)) { + debug("dropped rate loop sample"); + } + fast_rate_buffer->rate_decimation_count = 0; + fast_rate_buffer->_notifier.signal(); + return true; +} + +void AP_InertialSensor::update_backend_filters() +{ + for (uint8_t i=0; i<_backend_count; i++) { + _backends[i]->update_filters(); + } +} + +void AP_InertialSensor_Backend::update_filters() +{ + WITH_SEMAPHORE(_sem); + + update_accel_filters(accel_instance); + update_gyro_filters(gyro_instance); +} + +#endif // AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h new file mode 100644 index 0000000000..712500fb8d --- /dev/null +++ b/libraries/AP_InertialSensor/FastRateBuffer.h @@ -0,0 +1,50 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include "AP_InertialSensor.h" + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + +#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop + +#include +#include +#include +#include + +class FastRateBuffer +{ + friend class AP_InertialSensor; +public: + bool get_next_gyro_sample(Vector3f& gyro); + uint32_t get_num_gyro_samples() { return _rate_loop_gyro_window.available(); } + void set_rate_decimation(uint8_t rdec) { rate_decimation = rdec; } + // whether or not to push the current gyro sample + bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; } + bool gyro_samples_available() { return _rate_loop_gyro_window.available() > 0; } + +private: + /* + binary semaphore for rate loop to use to start a rate loop when + we hav finished filtering the primary IMU + */ + ObjectBuffer _rate_loop_gyro_window{AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE}; + uint8_t rate_decimation; // 0 means off + uint8_t rate_decimation_count; + HAL_BinarySemaphore _notifier; + HAL_Semaphore _mutex; +}; +#endif \ No newline at end of file From c823374986f9b93f6c3c2935a732d10550a1ceaa Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 5 Oct 2024 18:26:22 +0100 Subject: [PATCH 37/47] Copter: ensure decimated rates are never 0 in rate thread --- ArduCopter/rate_thread.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index 115c589729..ef8b624c54 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -152,7 +152,7 @@ uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz) { - return uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)); + return MAX(uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)), 1U); } static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count) From 398a70ec4b2a72aed6d3a2addf53a6c1c3a6bc6a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Oct 2024 13:04:27 +0900 Subject: [PATCH 38/47] AP_InertialSensor: avoid multiple allocations of rate loop buffer add nullptr checks and comments to FastRateBuffer --- .../AP_InertialSensor/AP_InertialSensor.h | 3 +- .../AP_InertialSensor/FastRateBuffer.cpp | 45 ++++++++++++++----- libraries/AP_InertialSensor/FastRateBuffer.h | 1 + 3 files changed, 36 insertions(+), 13 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 446a46f285..8cd1fd2307 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -806,6 +806,7 @@ private: // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED // Support for the fast rate thread in copter FastRateBuffer* fast_rate_buffer; + bool fast_rate_buffer_enabled; public: // enable the fast rate buffer and start pushing samples to it @@ -818,8 +819,6 @@ public: uint32_t get_num_gyro_samples(); // set the rate at which samples are collected, unused samples are dropped void set_rate_decimation(uint8_t rdec); - // are gyro samples being sourced from the rate loop buffer - bool use_rate_loop_gyro_samples() const; // push a new gyro sample into the fast rate buffer bool push_next_gyro_sample(const Vector3f& gyro); // run the filter parmeter update code. diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp index 6617915fec..c634c84d87 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.cpp +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -31,48 +31,62 @@ extern const AP_HAL::HAL& hal; #define debug(fmt, args ...) do {printf("IMU: " fmt "\n", ## args); } while(0) #endif +// enable the fast rate buffer and start pushing samples to it void AP_InertialSensor::enable_fast_rate_buffer() { - fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); + if (fast_rate_buffer_enabled) { + return; + } + if (fast_rate_buffer == nullptr) { + fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); + } + fast_rate_buffer_enabled = true; } +// disable the fast rate buffer and stop pushing samples to it void AP_InertialSensor::disable_fast_rate_buffer() { - delete fast_rate_buffer; - fast_rate_buffer = nullptr; + fast_rate_buffer_enabled = false; + if (fast_rate_buffer != nullptr) { + fast_rate_buffer->reset(); + } } +// get the number of available gyro samples in the fast rate buffer uint32_t AP_InertialSensor::get_num_gyro_samples() { + if (fast_rate_buffer == nullptr) { + return 0; + } return fast_rate_buffer->get_num_gyro_samples(); } +// set the rate at which samples are collected, unused samples are dropped void AP_InertialSensor::set_rate_decimation(uint8_t rdec) { + if (fast_rate_buffer == nullptr) { + return; + } fast_rate_buffer->set_rate_decimation(rdec); } -// are gyro samples being sourced from the rate loop buffer -bool AP_InertialSensor::use_rate_loop_gyro_samples() const -{ - return fast_rate_buffer != nullptr; -} - // whether or not to push the current gyro sample bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const { - return use_rate_loop_gyro_samples() && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); + return fast_rate_buffer_enabled && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); } +// get the next available gyro sample from the fast rate buffer bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) { - if (!use_rate_loop_gyro_samples()) { + if (!fast_rate_buffer_enabled) { return false; } return fast_rate_buffer->get_next_gyro_sample(gyro); } + bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) { if (!use_rate_loop_gyro_samples()) { @@ -88,8 +102,17 @@ bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) return _rate_loop_gyro_window.pop(gyro); } +void FastRateBuffer::reset() +{ + _rate_loop_gyro_window.clear(); +} + bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) { + if (fast_rate_buffer == nullptr) { + return false; + } + if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) { return false; } diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h index 712500fb8d..c4f53b7bf6 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.h +++ b/libraries/AP_InertialSensor/FastRateBuffer.h @@ -35,6 +35,7 @@ public: // whether or not to push the current gyro sample bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; } bool gyro_samples_available() { return _rate_loop_gyro_window.available() > 0; } + void reset(); private: /* From 8fc4a6a6e9a9ab6a46a924f483e765d0db33cebc Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Oct 2024 19:21:31 +0900 Subject: [PATCH 39/47] AP_HAL_ChibiOS: add comments to force_push on rcout --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 3 +++ libraries/AP_HAL_ChibiOS/RCOutput.h | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 33fbe4ac69..5da86d9558 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1399,6 +1399,9 @@ void RCOutput::trigger_groups() #if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED // trigger a PWM send if (!in_soft_serial() && + // we always trigger an output if we are in the main thread + // we also always trigger an output if we are in the rate thread and thus + // force_trigger has been set (hal.scheduler->in_main_thread() || force_trigger) && rcout_thread_ctx) { chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND); } diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index a935de7f45..c1ace28939 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -584,7 +584,7 @@ private: uint8_t _dshot_cycle; // virtual timer for post-push() pulses virtual_timer_t _dshot_rate_timer; - // force triggering of groups + // force triggering of groups, this is used by the rate thread to ensure output occurs bool force_trigger; #if HAL_DSHOT_ENABLED From 3d43f2053f81e47fd0664b95e12438584748100f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Oct 2024 13:04:08 +0900 Subject: [PATCH 40/47] Copter: move RTDT logging to fast path log after motor output in fast rate thread --- ArduCopter/Copter.cpp | 1 + ArduCopter/Copter.h | 1 + ArduCopter/Log.cpp | 38 ++++++++++++++++++++++++++++++++++++++ ArduCopter/Parameters.cpp | 4 ++-- ArduCopter/defines.h | 3 ++- ArduCopter/motors.cpp | 5 ++++- ArduCopter/rate_thread.cpp | 36 ++++++++++++++---------------------- 7 files changed, 62 insertions(+), 26 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a3ac456c8a..aee0028999 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -261,6 +261,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168), #endif #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + // don't delete this, there is an equivalent (virtual) in AP_Vehicle for the non-rate loop case SCHED_TASK(update_dynamic_notch_at_specified_rate_main, LOOP_RATE, 200, 215), #endif }; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index cbfbcef10e..491a2eb34c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -922,6 +922,7 @@ private: void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Vehicle_Startup_Messages(); + void Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin); #endif // HAL_LOGGING_ENABLED // mode.cpp diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index b446904e1a..2e06ecf80f 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -1,4 +1,5 @@ #include "Copter.h" +#include #if HAL_LOGGING_ENABLED @@ -343,6 +344,16 @@ struct PACKED log_Guided_Attitude_Target { float climb_rate; }; +// rate thread dt stats +struct PACKED log_Rate_Thread_Dt { + LOG_PACKET_HEADER; + uint64_t time_us; + float dt; + float dtAvg; + float dtMax; + float dtMin; +}; + // Write a Guided mode position target // pos_target is lat, lon, alt OR offset from ekf origin in cm // terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain @@ -390,6 +401,21 @@ void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, f logger.WriteBlock(&pkt, sizeof(pkt)); } +void Copter::Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin) +{ +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + const log_Rate_Thread_Dt pkt { + LOG_PACKET_HEADER_INIT(LOG_RATE_THREAD_DT_MSG), + time_us : AP_HAL::micros64(), + dt : dt, + dtAvg : dtAvg, + dtMax : dtMax, + dtMin : dtMin + }; + logger.WriteBlock(&pkt, sizeof(pkt)); +#endif +} + // type and unit information can be found in // libraries/AP_Logger/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -531,6 +557,18 @@ const struct LogStructure Copter::log_structure[] = { { LOG_GUIDED_ATTITUDE_TARGET_MSG, sizeof(log_Guided_Attitude_Target), "GUIA", "QBffffffff", "TimeUS,Type,Roll,Pitch,Yaw,RollRt,PitchRt,YawRt,Thrust,ClimbRt", "s-dddkkk-n", "F-000000-0" , true }, + +// @LoggerMessage: RTDT +// @Description: Attitude controller time deltas +// @Field: TimeUS: Time since system startup +// @Field: dt: current time delta +// @Field: dtAvg: current time delta average +// @Field: dtMax: Max time delta since last log output +// @Field: dtMin: Min time delta since last log output + + { LOG_RATE_THREAD_DT_MSG, sizeof(log_Rate_Thread_Dt), + "RTDT", "Qffff", "TimeUS,dt,dtAvg,dtMax,dtMin", "sssss", "F----" , true }, + }; uint8_t Copter::get_num_log_structures() const diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 37309440fb..a90c06b789 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1236,14 +1236,14 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED // @Param: FSTRATE_ENABLE // @DisplayName: Enable the fast Rate thread - // @Description: Enable the fast Rate thread + // @Description: Enable the fast Rate thread. In the default case the fast rate divisor, which controls the update frequency of the thread, is dynamically scaled from FSTRATE_DIV to avoid overrun in the gyro sample buffer and main loop slow-downs. Other values can be selected to fix the divisor to FSTRATE_DIV on arming or always. // @User: Advanced // @Values: 0:Disabled,1:Enabled-Dynamic,2:Enabled-FixedWhenArmed,3:Enabled-Fixed AP_GROUPINFO("FSTRATE_ENABLE", 9, ParametersG2, att_enable, 0), // @Param: FSTRATE_DIV // @DisplayName: Fast rate thread divisor - // @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate divided by this value. + // @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate in Hz divided by this value. This value is scaled depending on the configuration of FSTRATE_ENABLE. // @User: Advanced // @Range: 1 10 AP_GROUPINFO("FSTRATE_DIV", 10, ParametersG2, att_decimation, 1), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index acba81d953..6603c1846e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -87,7 +87,8 @@ enum LoggingParameters { LOG_GUIDED_POSITION_TARGET_MSG, LOG_SYSIDD_MSG, LOG_SYSIDS_MSG, - LOG_GUIDED_ATTITUDE_TARGET_MSG + LOG_GUIDED_ATTITUDE_TARGET_MSG, + LOG_RATE_THREAD_DT_MSG }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 784580b8ec..0b2193107f 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -133,6 +133,7 @@ void Copter::auto_disarm_check() } // motors_output - send output to motors library which will adjust and send to ESCs and servos +// full_push is true when slower rate updates (e.g. servo output) need to be performed at the main loop rate. void Copter::motors_output(bool full_push) { #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED @@ -184,13 +185,15 @@ void Copter::motors_output(bool full_push) // push all channels if (full_push) { + // motor output including servos and other updates that need to run at the main loop rate srv.push(); } else { + // motor output only at main loop rate or faster hal.rcout->push(); } } -// motors_output from main thread +// motors_output from main thread at main loop rate void Copter::motors_output_main() { if (!using_rate_thread) { diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index ef8b624c54..ac533165e3 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -248,28 +248,6 @@ void Copter::rate_controller_thread() const float dt = dt_us * 1.0e-6; last_run_us = now_us; - max_dt = MAX(dt, max_dt); - min_dt = MIN(dt, min_dt); - -#if HAL_LOGGING_ENABLED -// @LoggerMessage: RTDT -// @Description: Attitude controller time deltas -// @Field: TimeUS: Time since system startup -// @Field: dt: current time delta -// @Field: dtAvg: current time delta average -// @Field: dtMax: Max time delta since last log output -// @Field: dtMin: Min time delta since last log output - - if (now_ms - last_rtdt_log_ms >= 10) { // 100 Hz - AP::logger().WriteStreaming("RTDT", "TimeUS,dt,dtAvg,dtMax,dtMin", "Qffff", - AP_HAL::micros64(), - dt, sensor_dt, max_dt, min_dt); - max_dt = sensor_dt; - min_dt = sensor_dt; - last_rtdt_log_ms = now_ms; - } -#endif - motors->set_dt(sensor_dt); // check if we are falling behind if (ins.get_num_gyro_samples() > 2) { @@ -304,6 +282,18 @@ void Copter::rate_controller_thread() rate_controller_filter_update(); } + max_dt = MAX(dt, max_dt); + min_dt = MIN(dt, min_dt); + +#if HAL_LOGGING_ENABLED + if (now_ms - last_rtdt_log_ms >= 100) { // 10 Hz + Log_Write_Rate_Thread_Dt(dt, sensor_dt, max_dt, min_dt); + max_dt = sensor_dt; + min_dt = sensor_dt; + last_rtdt_log_ms = now_ms; + } +#endif + #ifdef RATE_LOOP_TIMING_DEBUG motor_output_us += AP_HAL::micros() - rate_now_us; rate_now_us = AP_HAL::micros(); @@ -453,6 +443,7 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControlle return 0; } +// enable the fast rate thread using the provided decimation rate and record the new output rates void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates) { ins.enable_fast_rate_buffer(); @@ -461,6 +452,7 @@ void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& using_rate_thread = true; } +// disable the fast rate thread and record the new output rates void Copter::disable_fast_rate_loop(RateControllerRates& rates) { using_rate_thread = false; From 96a70df75be93ec52d41307b36d7e16c4680d9a3 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 10 Nov 2024 14:34:41 +0000 Subject: [PATCH 41/47] Copter: address review comments --- ArduCopter/Copter.h | 2 +- ArduCopter/rate_thread.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 491a2eb34c..a94edddef2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -753,7 +753,7 @@ private: void rate_controller_thread(); void rate_controller_filter_update(); void rate_controller_log_update(); - uint8_t rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high); + void rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high); void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates); void disable_fast_rate_loop(RateControllerRates& rates); void update_dynamic_notch_at_specified_rate_main(); diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index ac533165e3..5b64301fb7 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -248,7 +248,6 @@ void Copter::rate_controller_thread() const float dt = dt_us * 1.0e-6; last_run_us = now_us; - motors->set_dt(sensor_dt); // check if we are falling behind if (ins.get_num_gyro_samples() > 2) { running_slow++; @@ -420,7 +419,7 @@ void Copter::rate_controller_filter_update() /* update rate controller rates and return the logging rate */ -uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high) +void Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high) { const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation; attitude_control->set_notch_sample_rate(attitude_rate); @@ -439,8 +438,6 @@ uint8_t Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControlle #endif rates.main_loop_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz()); rates.filter_rate = calc_gyro_decimation(rate_decimation, ins.get_raw_gyro_rate_hz() / 2); - - return 0; } // enable the fast rate thread using the provided decimation rate and record the new output rates From 4b0fa15278ca98aee1e34f1bbae0040c957c25e2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 10 Nov 2024 14:34:55 +0000 Subject: [PATCH 42/47] AP_InertialSensor: address review comments --- libraries/AP_InertialSensor/AP_InertialSensor.h | 2 -- libraries/AP_InertialSensor/FastRateBuffer.h | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 8cd1fd2307..bae72d86f9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -9,8 +9,6 @@ #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout -#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop - #include #include diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h index c4f53b7bf6..77ce3e2679 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.h +++ b/libraries/AP_InertialSensor/FastRateBuffer.h @@ -48,4 +48,4 @@ private: HAL_BinarySemaphore _notifier; HAL_Semaphore _mutex; }; -#endif \ No newline at end of file +#endif From 277386e6e2888ef127dc93227d492684a5bd1546 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 13 Nov 2024 13:30:29 +0000 Subject: [PATCH 43/47] AP_HAL_ChibiOS: add cork()/push() check --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 5da86d9558..5d4274ac70 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1329,6 +1329,9 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) */ void RCOutput::cork(void) { + if (corked) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } corked = true; #if HAL_WITH_IO_MCU if (iomcu_enabled) { @@ -1342,6 +1345,9 @@ void RCOutput::cork(void) */ void RCOutput::push(void) { + if (!corked) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } corked = false; push_local(); #if HAL_WITH_IO_MCU From 88f6125a942df9dd9a283cfbc5d57d50c25545b1 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 16 Nov 2024 16:30:46 +0000 Subject: [PATCH 44/47] AP_InertialSensor: belt and braces checks --- libraries/AP_InertialSensor/FastRateBuffer.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp index c634c84d87..29601965d3 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.cpp +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -40,7 +40,7 @@ void AP_InertialSensor::enable_fast_rate_buffer() if (fast_rate_buffer == nullptr) { fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); } - fast_rate_buffer_enabled = true; + fast_rate_buffer_enabled = fast_rate_buffer != nullptr; } // disable the fast rate buffer and stop pushing samples to it @@ -55,7 +55,7 @@ void AP_InertialSensor::disable_fast_rate_buffer() // get the number of available gyro samples in the fast rate buffer uint32_t AP_InertialSensor::get_num_gyro_samples() { - if (fast_rate_buffer == nullptr) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return 0; } return fast_rate_buffer->get_num_gyro_samples(); @@ -64,7 +64,7 @@ uint32_t AP_InertialSensor::get_num_gyro_samples() // set the rate at which samples are collected, unused samples are dropped void AP_InertialSensor::set_rate_decimation(uint8_t rdec) { - if (fast_rate_buffer == nullptr) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return; } fast_rate_buffer->set_rate_decimation(rdec); @@ -73,13 +73,16 @@ void AP_InertialSensor::set_rate_decimation(uint8_t rdec) // whether or not to push the current gyro sample bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const { - return fast_rate_buffer_enabled && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { + return false; + } + return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); } // get the next available gyro sample from the fast rate buffer bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) { - if (!fast_rate_buffer_enabled) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return false; } @@ -109,7 +112,7 @@ void FastRateBuffer::reset() bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) { - if (fast_rate_buffer == nullptr) { + if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return false; } From 3ed6ada2e316c52ee9ebae4202c99c681e891693 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 16 Nov 2024 16:31:21 +0000 Subject: [PATCH 45/47] Copter: allocation failure on rate thread start --- ArduCopter/Copter.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index aee0028999..c493eda291 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -817,6 +817,8 @@ void Copter::one_hz_loop() "rate", 1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { started_rate_thread = true; + } else { + AP_BoardConfig::allocation_error("rate thread"); } } #endif From 768b2eabc48ff2da4053745ffe489d10ea4b4e29 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 22 Nov 2024 11:36:22 +0100 Subject: [PATCH 46/47] autotest: Created test for unsuccessful takeoff level off --- .../TakeoffBadLevelOff/flaps_tkoff_50.txt | 13 ++++++ Tools/autotest/arduplane.py | 41 +++++++++++++++++++ 2 files changed, 54 insertions(+) create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt b/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt new file mode 100644 index 0000000000..c2f52f8929 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 50.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 0ce13e5c3e..7fabd6cd07 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4860,6 +4860,46 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.fly_home_land_and_disarm() + def TakeoffBadLevelOff(self): + '''Ensure that the takeoff can be completed under 0 pitch demand.''' + ''' + When using no airspeed, the pitch level-off will eventually command 0 + pitch demand. Ensure that the plane can climb the final 2m to deem the + takeoff complete. + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "PTCH_TRIM_DEG": -10.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + "TKOFF_ALT": 50.0, + "TKOFF_DIST": 1000.0, + "TKOFF_THR_MAX": 75.0, + "TKOFF_THR_MINACC": 3.0, + }) + + self.load_mission("flaps_tkoff_50.txt") + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we've reached the takeoff altitude. + target_alt = 50 + self.wait_altitude(target_alt-1, target_alt+1, relative=True, timeout=30) + + self.delay_sim_time(5) + + self.disarm_vehicle(force=True) + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -6452,6 +6492,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.TakeoffTakeoff4, self.TakeoffGround, self.TakeoffIdleThrottle, + self.TakeoffBadLevelOff, self.ForcedDCM, self.DCMFallback, self.MAVFTP, From 9a5f81aa95e036c5dfc6a772d8d2f492a7e7241f Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 22 Nov 2024 15:36:46 +0100 Subject: [PATCH 47/47] Plane: Added check for takeoff level-off timeout When an airspeed sensor is not used, during a takeoff, the pitch angle is asymptotically driven to 0 as the takeoff altitude is approached. Some airplanes will then stop climbing and fail to reach altitude. To prevent an indefinite wait for the takeoff altitude to be reached, a dedicated level-off timeout has been introduced. --- ArduPlane/Plane.h | 3 ++- ArduPlane/commands_logic.cpp | 5 ++++- ArduPlane/mode_takeoff.cpp | 7 +++++++ ArduPlane/takeoff.cpp | 19 +++++++++++++++++-- 4 files changed, 30 insertions(+), 4 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d2d35a7b14..df180b6f74 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -453,7 +453,7 @@ private: float throttle_lim_max; float throttle_lim_min; uint32_t throttle_max_timer_ms; - // Good candidate for keeping the initial time for TKOFF_THR_MAX_T. + uint32_t level_off_start_time_ms; } takeoff_state; // ground steering controller state @@ -1147,6 +1147,7 @@ private: int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); bool check_takeoff_timeout(void); + bool check_takeoff_timeout_level_off(void); // avoidance_adsb.cpp void avoidance_adsb_update(void); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 2110b1455d..624dc2749c 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -589,7 +589,10 @@ bool Plane::verify_takeoff() // see if we have reached takeoff altitude int32_t relative_alt_cm = adjusted_relative_altitude_cm(); - if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { + if ( + relative_alt_cm > auto_state.takeoff_altitude_rel_cm || // altitude reached + plane.check_takeoff_timeout_level_off() // pitch level-off maneuver has timed out + ) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm", (double)(relative_alt_cm*0.01f)); steer_state.hold_course_cd = -1; diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 5f93cc4729..d8e3c11f75 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -119,6 +119,7 @@ void ModeTakeoff::update() gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); plane.takeoff_state.start_time_ms = millis(); + plane.takeoff_state.level_off_start_time_ms = 0; plane.takeoff_state.throttle_max_timer_ms = millis(); takeoff_mode_setup = true; plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. @@ -157,6 +158,12 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } + // If we have timed-out on the attempt to close the final few meters + // during pitch level-off, continue to NORMAL flight stage. + if (plane.check_takeoff_timeout_level_off()) { + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { //below TKOFF_ALT plane.takeoff_calc_roll(); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index dda412d5ee..d7617e2f56 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void) takeoff_state.launchTimerStarted = false; takeoff_state.last_tkoff_arm_time = 0; takeoff_state.start_time_ms = now; + takeoff_state.level_off_start_time_ms = 0; takeoff_state.throttle_max_timer_ms = now; steer_state.locked_course_err = 0; // use current heading without any error offset return true; @@ -316,6 +317,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) // make a note of that altitude to use it as a start height for scaling gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100)); auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm; + takeoff_state.level_off_start_time_ms = AP_HAL::millis(); } } } @@ -376,9 +378,8 @@ void Plane::landing_gear_update(void) #endif /* - check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout + check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred */ - bool Plane::check_takeoff_timeout(void) { if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { @@ -400,3 +401,17 @@ bool Plane::check_takeoff_timeout(void) return false; } +/* + check if the pitch level-off time has expired; returns true if timeout has occurred +*/ +bool Plane::check_takeoff_timeout_level_off(void) +{ + if (takeoff_state.level_off_start_time_ms > 0) { + // A takeoff is in progress. + uint32_t now = AP_HAL::millis(); + if ((now - takeoff_state.level_off_start_time_ms) > (uint32_t)(1000U * g.takeoff_pitch_limit_reduction_sec)) { + return true; + } + } + return false; +}