From a75f1f77d70970008ceb9c6a13ba0e98843dab4e Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Fri, 27 May 2022 14:03:52 +1000 Subject: [PATCH 01/48] AP_BattMonitor: AD7091R5 File addition new Battery Type This is a new battery type which reads data of 2 battery over I2C using an ADC I2C Extender. --- .../AP_BattMonitor_AD7091R5.cpp | 236 ++++++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_AD7091R5.h | 75 ++++++ 2 files changed, 311 insertions(+) create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp new file mode 100644 index 0000000000..8f09bf9beb --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp @@ -0,0 +1,236 @@ +#include "AP_BattMonitor_AD7091R5.h" + +/** + * @brief You can use it to Read Current and voltage of 1-3 batteries from a ADC extender IC over I2C. + * AD7091R5 is a ADC extender and we are using it to read current and voltage of multiple batteries. + * Examples of Pin combination: + * 1)Pin 50 = Voltage 51,52,53 = Current. For 3 battery combination Voltage will be same accross. + * 2)Pin 50,51 = Voltage and Current Battery 1 - Pin 52,53 = Voltage and Current Battery 2 + * Only the First instance of Battery Monitor will be reading the values from IC over I2C. + * Make sure you understand the method of calculation used in this driver before using it. + * e.g. using pin 1 on IC to read voltage of 2 batteries and pin 2 and 3 to read current from individual battery. + * Pin number represents 50 = pin 1, 51 = pin 2 and so on 52, 53 + * BATT2_Monitor = 24 , BATT3_Monitor = 24 + * BATT2_VOLT_PIN = 50 , BATT3_VOLT_PIN = 50 + * BATT2_CURR_PIN = 51 , BATT3_CURR_PIN = 52 + * + * + */ + +#if AP_BATTMON_AD7091R5_ENABLED + +#include +#include +#include + +//macro defines +#define AD7091R5_I2C_ADDR 0x2F // A0 and A1 tied to GND +#define AD7091R5_I2C_BUS 0 +#define AD7091R5_RESET 0x02 +#define AD7091R5_RESULT_ADDR 0x00 +#define AD7091R5_CHAN_ADDR 0x01 +#define AD7091R5_CONF_ADDR 0x02 +#define AD7091R5_CH_ID(x) ((x >> 5) & 0x03) +#define AD7091R5_RES_MASK 0x0F +#define AD7091R5_REF 3.3f +#define AD7091R5_RESOLUTION (float)4096 +#define AD7091R5_PERIOD_USEC 100000 +#define AD7091R5_BASE_PIN 50 + + +extern const AP_HAL::HAL& hal; +const AP_Param::GroupInfo AP_BattMonitor_AD7091R5::var_info[] = { + + // @Param: VOLT_PIN + // @DisplayName: Battery Voltage sensing pin on the AD7091R5 Ic + // @Description: Sets the analog input pin that should be used for voltage monitoring on AD7091R5. + // @Values: -1:Disabled + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("VOLT_PIN", 30, AP_BattMonitor_AD7091R5, _volt_pin, 0), + + // @Param: CURR_PIN + // @DisplayName: Battery Current sensing pin + // @Description: Sets the analog input pin that should be used for Current monitoring on AD7091R5. + // @Values: -1:Disabled + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("CURR_PIN", 31, AP_BattMonitor_AD7091R5, _curr_pin, 0), + + // @Param: VOLT_MULT + // @DisplayName: Voltage Multiplier + // @Description: Used to convert the voltage of the voltage sensing pin (@PREFIX@VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). + // @User: Advanced + AP_GROUPINFO("VOLT_MULT", 32, AP_BattMonitor_AD7091R5, _volt_multiplier, 0), + + // @Param: AMP_PERVLT + // @DisplayName: Amps per volt + // @Description: Number of amps that a 1V reading on the current sensor corresponds to. + // @Units: A/V + // @User: Standard + AP_GROUPINFO("AMP_PERVLT", 33, AP_BattMonitor_AD7091R5, _curr_amp_per_volt, 0), + + // @Param: AMP_OFFSET + // @DisplayName: AMP offset + // @Description: Voltage offset at zero current on current sensor + // @Units: V + // @User: Standard + AP_GROUPINFO("AMP_OFFSET", 34, AP_BattMonitor_AD7091R5, _curr_amp_offset, 0), + + // @Param: VLT_OFFSET + // @DisplayName: Volage offset + // @Description: Voltage offset on voltage pin. This allows for an offset due to a diode. This voltage is subtracted before the scaling is applied + // @Units: V + // @User: Advanced + AP_GROUPINFO("VLT_OFFSET", 35, AP_BattMonitor_AD7091R5, _volt_offset, 0), + + // Param indexes must be less than 10 to avoid conflict with other battery monitor param tables loaded by pointer + + AP_GROUPEND +}; + + +//Variable initialised to read from first instance. +AP_BattMonitor_AD7091R5::AnalogData AP_BattMonitor_AD7091R5::_analog_data[AD7091R5_NO_OF_CHANNELS] = {0}; +bool AP_BattMonitor_AD7091R5::_first = true; +bool AP_BattMonitor_AD7091R5::_health = false; + +/** + * @brief Construct a new ap battmonitor ad7091r5::ap battmonitor ad7091r5 object + * + * @param mon + * @param mon_state + * @param params + */ +AP_BattMonitor_AD7091R5::AP_BattMonitor_AD7091R5(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; +} + +/** + * @brief probe and initialize the sensor and register call back + * + */ +void AP_BattMonitor_AD7091R5::init() +{ + // voltage and current pins from params and check if there are in range + if (_volt_pin.get() >= AD7091R5_BASE_PIN && _volt_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS && + _curr_pin.get() >= AD7091R5_BASE_PIN && _curr_pin.get() <= AD7091R5_BASE_PIN + AD7091R5_NO_OF_CHANNELS) { + volt_buff_pt = _volt_pin.get() - AD7091R5_BASE_PIN; + curr_buff_pt = _curr_pin.get() - AD7091R5_BASE_PIN; + } + else{ + return; //pin values are out of range + } + + // only the first instance read the i2c device + if (_first) { + _first = false; + // probe i2c device + _dev = hal.i2c_mgr->get_device(AD7091R5_I2C_BUS, AD7091R5_I2C_ADDR); + + if (_dev) { + WITH_SEMAPHORE(_dev->get_semaphore()); + _dev->set_retries(10); // lots of retries during probe + //Reset and config device + if (_initialize()) { + _dev->set_retries(2); // drop to 2 retries for runtime + _dev->register_periodic_callback(AD7091R5_PERIOD_USEC, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_AD7091R5::_read_adc, void)); + } + } + } +} + +/** + * @brief read - read the voltage and curren + * + */ +void AP_BattMonitor_AD7091R5::read() +{ + + WITH_SEMAPHORE(sem); + //copy global health status to all instances + _state.healthy = _health; + + //return if system not healthy + if (!_state.healthy) { + return; + } + + //voltage conversion + _state.voltage = (_data_to_volt(_analog_data[volt_buff_pt].data) - _volt_offset) * _volt_multiplier; + + //current amps conversion + _state.current_amps = (_data_to_volt(_analog_data[curr_buff_pt].data) - _curr_amp_offset) * _curr_amp_per_volt; + + // calculate time since last current read + uint32_t tnow = AP_HAL::micros(); + uint32_t dt_us = tnow - _state.last_time_micros; + + // update total current drawn since startup + update_consumed(_state, dt_us); + + // record time + _state.last_time_micros = tnow; +} + +/** + * @brief read all four channels and store the results + * + */ +void AP_BattMonitor_AD7091R5::_read_adc() +{ + uint8_t data[AD7091R5_NO_OF_CHANNELS*2]; + //reset and reconfigure IC if health status is not good. + if (!_state.healthy) { + _initialize(); + } + //read value + bool ret = _dev->transfer(nullptr, 0, data, sizeof(data)); + WITH_SEMAPHORE(sem); + if (ret) { + for (int i=0; itransfer(data, sizeof(data), nullptr, 0)){ + //command mode, use external 3.3 reference, all channels enabled, set address pointer register to read the adc results + uint8_t data_2[6] = {AD7091R5_CONF_ADDR, AD7091R5_CONF_CMD, AD7091R5_CONF_PDOWN0, AD7091R5_CHAN_ADDR, AD7091R5_CHAN_ALL, AD7091R5_RESULT_ADDR}; + return _dev->transfer(data_2, sizeof(data_2), nullptr, 0); + } + return false; +} + +/** + * @brief convert binary reading to volts + * + * @param data + * @return float + */ +float AP_BattMonitor_AD7091R5::_data_to_volt(uint32_t data) +{ + return (AD7091R5_REF/AD7091R5_RESOLUTION)*data; +} + +#endif // AP_BATTMON_AD7091R5_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h new file mode 100644 index 0000000000..eab1f6d2f0 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.h @@ -0,0 +1,75 @@ +#pragma once + +#include "AP_BattMonitor_Backend.h" + +#ifndef AP_BATTMON_AD7091R5_ENABLED +#define AP_BATTMON_AD7091R5_ENABLED (BOARD_FLASH_SIZE > 1024) +#endif + +#if AP_BATTMON_AD7091R5_ENABLED + +#include +#include + +#define AD7091R5_NO_OF_CHANNELS 4 +#define AD7091R5_CONF_CMD 0x04 +#define AD7091R5_CHAN_ALL 0x0F +#define AD7091R5_CONF_PDOWN0 0x00 +#define AD7091R5_CONF_PDOWN2 0x02 +#define AD7091R5_CONF_PDOWN3 0x03 +#define AD7091R5_CONF_PDOWN_MASK 0x03 + +class AP_BattMonitor_AD7091R5 : public AP_BattMonitor_Backend +{ +public: + // Constructor + AP_BattMonitor_AD7091R5(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms); + + // Read the battery voltage and current. Should be called at 10hz + void read() override; + void init(void) override; + + // returns true if battery monitor provides consumed energy info + bool has_consumed_energy() const override + { + return has_current(); + } + + // returns true if battery monitor provides current info + bool has_current() const override + { + return true; + } + + static const struct AP_Param::GroupInfo var_info[]; + +private: + void _read_adc(); + bool _initialize(); + float _data_to_volt(uint32_t data); + + static struct AnalogData { + uint32_t data; + } _analog_data[AD7091R5_NO_OF_CHANNELS]; + static bool _first; + static bool _health; + + HAL_Semaphore sem; // semaphore for access to shared frontend data + AP_HAL::OwnPtr _dev; + uint8_t volt_buff_pt; + uint8_t curr_buff_pt; + +protected: + + // Parameters + AP_Float _volt_multiplier; // voltage on volt pin multiplied by this to calculate battery voltage + AP_Float _curr_amp_per_volt; // voltage on current pin multiplied by this to calculate current in amps + AP_Float _curr_amp_offset; // offset voltage that is subtracted from current pin before conversion to amps + AP_Float _volt_offset; // offset voltage that is subtracted from voltage pin before conversion + AP_Int8 _volt_pin; // board pin used to measure battery voltage + AP_Int8 _curr_pin; // board pin used to measure battery current +}; + +#endif // AP_BATTMON_AD7091R5_ENABLED From da58b3e5fd994cede4548e67f04e18a3a01b9768 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 18 Apr 2023 12:22:52 +1000 Subject: [PATCH 02/48] AP_BattMonitor: Addition AD7091R5 Battery Type Addition of Battery Type. --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 6 ++++++ libraries/AP_BattMonitor/AP_BattMonitor.h | 4 +++- libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index e125bbd0cc..4fa46ecb0b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -16,6 +16,7 @@ #include "AP_BattMonitor_INA2xx.h" #include "AP_BattMonitor_LTC2946.h" #include "AP_BattMonitor_Torqeedo.h" +#include "AP_BattMonitor_AD7091R5.h" #include "AP_BattMonitor_FuelLevel_Analog.h" #include @@ -323,6 +324,11 @@ AP_BattMonitor::init() case Type::Torqeedo: drivers[instance] = new AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]); break; +#endif +#if AP_BATTMON_AD7091R5_ENABLED + case Type::AD7091R5: + drivers[instance] = new AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]); + break; #endif case Type::NONE: default: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index f4eaa68518..085d676c11 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -57,6 +57,7 @@ class AP_BattMonitor_INA2XX; class AP_BattMonitor_LTC2946; class AP_BattMonitor_Torqeedo; class AP_BattMonitor_FuelLevel_Analog; +class AP_BattMonitor_AD7091R5; class AP_BattMonitor { @@ -76,7 +77,7 @@ class AP_BattMonitor friend class AP_BattMonitor_LTC2946; friend class AP_BattMonitor_Torqeedo; - friend class AP_BattMonitor_FuelLevel_Analog; + friend class AP_BattMonitor_AD7091R5; public: @@ -112,6 +113,7 @@ class AP_BattMonitor LTC2946 = 22, Torqeedo = 23, FuelLevel_Analog = 24, + AD7091R5 = 28, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 87b9a2a541..6c4f60b146 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -13,7 +13,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 + // @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:AD7091R5 // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE), From c35cf466b38da8b262f4d56a17aeb14432012d2b Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 18 Apr 2023 12:50:50 +1000 Subject: [PATCH 03/48] AP_Periph: add arm data receive monitoring Currently if the CAN data is stopped receiving AP_Periph will still remain in the last known ARM state. Changes Made: AP_Periph will Disarm if 1) A specific disarm command is received 2) no arming command and no ESC command receive for DISARM_DELAY period. --- Tools/AP_Periph/AP_Periph.cpp | 14 +++++++++++++- Tools/AP_Periph/AP_Periph.h | 3 +++ Tools/AP_Periph/Parameters.cpp | 3 +++ Tools/AP_Periph/Parameters.h | 6 ++++++ Tools/AP_Periph/can.cpp | 6 ++++++ .../AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 5 +++++ 6 files changed, 36 insertions(+), 1 deletion(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 4d228cdab3..d5df593d7f 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -24,6 +24,7 @@ #include #include "AP_Periph.h" #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include @@ -386,7 +387,18 @@ void AP_Periph_FW::update() last_error_ms = now; can_printf("IERR 0x%x %u", unsigned(ierr.errors()), unsigned(ierr.last_error_line())); } - +#if HAL_PERIPH_ARM_MONITORING_ENABLE + static uint32_t last_arm_check_ms; + if (now - last_arm_check_ms > g.disarm_delay){ + last_arm_check_ms = now; + if(periph.arm_update_status){ + periph.arm_update_status = false; + } + else{ + hal.util->set_soft_armed(UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED); + } + } +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE static uint32_t last_debug_ms; if ((g.debug&(1< 5000) { diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 85f356659d..8e65a8b3f4 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -256,6 +256,9 @@ class AP_Periph_FW { #if HAL_GCS_ENABLED GCS_Periph _gcs; +#endif +#if HAL_PERIPH_ARM_MONITORING_ENABLE + bool arm_update_status; #endif // setup the var_info table AP_Param param_loader{var_info}; diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 394a77a6fe..e00475fffd 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -451,6 +451,9 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(efi, "EFI", AP_EFI), #endif +#if HAL_PERIPH_ARM_MONITORING_ENABLE + GSCALAR(disarm_delay, "DISARM_DELAY", 1000), +#endif AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 092b4106ad..1d12ccbeae 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -59,6 +59,7 @@ class Parameters { k_param_efi, k_param_efi_port, k_param_efi_baudrate, + k_param_disarm_delay, }; AP_Int16 format_version; @@ -143,6 +144,11 @@ class Parameters { #else static constexpr uint8_t can_fdmode = 0; #endif + +#if HAL_PERIPH_ARM_MONITORING_ENABLE + AP_Int32 disarm_delay; +#endif + Parameters() {} }; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index d7759bda54..0b0745fcf9 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -607,6 +607,9 @@ static void handle_arming_status(CanardInstance* ins, CanardRxTransfer* transfer return; } hal.util->set_soft_armed(req.status == UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED); +#if HAL_PERIPH_ARM_MONITORING_ENABLE + periph.arm_update_status = true; +#endif } #ifdef HAL_PERIPH_ENABLE_GPS @@ -762,6 +765,9 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe return; } periph.rcout_esc(cmd.cmd.data, cmd.cmd.len); +#if HAL_PERIPH_ARM_MONITORING_ENABLE + periph.arm_update_status = true; +#endif } static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index f3acfb703b..8d220b0559 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -2867,6 +2867,10 @@ def add_apperiph_defaults(f): #define AP_STATS_ENABLED 0 #endif +#ifndef HAL_PERIPH_ARM_MONITORING_ENABLE +#define HAL_PERIPH_ARM_MONITORING_ENABLE 0 +#endif + /* * GPS Backends - we selectively turn backends on. * Note also that f103-GPS explicitly disables some of these backends. @@ -2877,6 +2881,7 @@ def add_apperiph_defaults(f): #define AP_GPS_ERB_ENABLED 0 #endif + #ifndef AP_GPS_GSOF_ENABLED #define AP_GPS_GSOF_ENABLED defined(HAL_PERIPH_ENABLE_GPS) #endif From 5762ef2094329b66fedb9f5494ff1796f19deaa5 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 31 May 2022 18:17:49 +1000 Subject: [PATCH 04/48] Hwdef: CarbonixF405 enable arm monitoring --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index 011bf96f83..1a7a7ca53e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -136,6 +136,8 @@ define HAL_PERIPH_ENABLE_ADSB define HAL_PERIPH_ADSB_PORT_DEFAULT 3 define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 +define HAL_PERIPH_ARM_MONITORING_ENABLE 1 + BARO MS56XX I2C:0:0x76 COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE From bb18780a38347478b407fcca1f8b531b51ee14aa Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 1 Jun 2022 21:23:59 +1000 Subject: [PATCH 05/48] AP_Compass : Add QMC5883P --- libraries/AP_Compass/AP_Compass.cpp | 19 ++ libraries/AP_Compass/AP_Compass.h | 1 + libraries/AP_Compass/AP_Compass_Backend.h | 1 + libraries/AP_Compass/AP_Compass_QMC5883P.cpp | 218 +++++++++++++++++++ libraries/AP_Compass/AP_Compass_QMC5883P.h | 68 ++++++ 5 files changed, 307 insertions(+) create mode 100644 libraries/AP_Compass/AP_Compass_QMC5883P.cpp create mode 100644 libraries/AP_Compass/AP_Compass_QMC5883P.h diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index e4067c7062..de84764728 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -21,6 +21,7 @@ #include "AP_Compass_LIS3MDL.h" #include "AP_Compass_AK09916.h" #include "AP_Compass_QMC5883L.h" +#include "AP_Compass_QMC5883P.h" #if HAL_ENABLE_LIBUAVCAN_DRIVERS #include "AP_Compass_UAVCAN.h" #endif @@ -1094,6 +1095,24 @@ void Compass::_probe_external_i2c_compasses(void) } #endif +#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_QMC5883P) + //external i2c bus + FOREACH_I2C_EXTERNAL(i) { + ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR), + true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL)); + } + + // internal i2c bus + if (all_external) { + // only probe QMC5883P on internal if we are treating internals as externals + FOREACH_I2C_INTERNAL(i) { + ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR), + all_external, + all_external?HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL)); + } + } +#endif + #ifndef HAL_BUILD_AP_PERIPH // AK09916 on ICM20948 FOREACH_I2C_EXTERNAL(i) { diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index d2040c1434..c7cc075338 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -432,6 +432,7 @@ friend class AP_Compass_Backend; DRIVER_MSP =17, DRIVER_SERIAL =18, DRIVER_MMC5XX3 =19, + DRIVER_QMC5883P =20, }; bool _driver_enabled(enum DriverType driver_type); diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 8636f98258..e014978ea5 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -68,6 +68,7 @@ class AP_Compass_Backend DEVTYPE_MMC5983 = 0x13, DEVTYPE_AK09918 = 0x14, DEVTYPE_AK09915 = 0x15, + DEVTYPE_QMC5883P = 0x16, }; #if HAL_MSP_COMPASS_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp new file mode 100644 index 0000000000..6fe1a718ca --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp @@ -0,0 +1,218 @@ +/* + * This file 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 file 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 . + * + * Driver by Lokesh Ramina, Jan 2022 + */ +#include "AP_Compass_QMC5883P.h" + +#include + +#include +#include + + +//Register Address +#define QMC5883P_REG_ID 0x00 +#define QMC5883P_REG_DATA_OUTPUT_X 0x01 +#define QMC5883P_REG_DATA_OUTPUT_Z_MSB 0x06 +#define QMC5883P_REG_STATUS 0x09 +#define QMC5883P_REG_CONF1 0x0A +#define QMC5883P_REG_CONF2 0x0B + +#define QMC5883P_ID_VAL 0x80 + +//Register values +// Sensor operation modes +#define QMC5883P_MODE_SUSPEND 0x00 +#define QMC5883P_MODE_NORMAL 0x01 +#define QMC5883P_MODE_SINGLE 0x02 +#define QMC5883P_MODE_CONTINUOUS 0x03 + +// ODR data output rates for 5883L +#define QMC5883P_ODR_10HZ (0x00 << 2) +#define QMC5883P_ODR_50HZ (0x01 << 2) +#define QMC5883P_ODR_100HZ (0x02 << 2) +#define QMC5883P_ODR_200HZ (0x03 << 2) + +// Over sampling Ratio OSR1 +#define QMC5883P_OSR1_8 (0x00 << 4) +#define QMC5883P_OSR1_4 (0x01 << 4) +#define QMC5883P_OSR1_2 (0x02 << 4) +#define QMC5883P_OSR1_1 (0x03 << 4) + +// Down sampling Rate OSR2 +#define QMC5883P_OSR2_8 0x08 + +//RNG +#define QMC5883P_RNG_30G (0x00 << 2) +#define QMC5883P_RNG_12G (0x01 << 2) +#define QMC5883P_RNG_8G (0x10 << 2) +#define QMC5883P_RNG_2G (0x11 << 2) + +#define QMC5883P_SET_XYZ_SIGN 0x29 + +//Reset +#define QMC5883P_RST 0x80 + +//Status Val +#define QMC5883P_STATUS_DATA_READY 0x01 + +#ifndef DEBUG +#define DEBUG 0 +#endif + +extern const AP_HAL::HAL &hal; + +AP_Compass_Backend *AP_Compass_QMC5883P::probe(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation) +{ + if (!dev) { + return nullptr; + } + AP_Compass_QMC5883P *sensor = new AP_Compass_QMC5883P(std::move(dev),force_external,rotation); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +AP_Compass_QMC5883P::AP_Compass_QMC5883P(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation) + : _dev(std::move(dev)) + , _rotation(rotation) + , _force_external(force_external) +{ +} + +bool AP_Compass_QMC5883P::init() +{ + _dev->get_semaphore()->take_blocking(); + + _dev->set_retries(10); +#if DEBUG + _dump_registers(); +#endif + if (!_check_whoami()) { + goto fail; + } + //As mentioned in the Datasheet 7.2 to do continues mode 0x29 will set sign for X,Y,Z + if (!_dev->write_register(QMC5883P_REG_DATA_OUTPUT_Z_MSB, QMC5883P_SET_XYZ_SIGN)|| + !_dev->write_register(QMC5883P_REG_CONF1, + QMC5883P_MODE_CONTINUOUS| + QMC5883P_ODR_100HZ| + QMC5883P_OSR1_8| + QMC5883P_OSR2_8)|| + !_dev->write_register(QMC5883P_REG_CONF2,QMC5883P_OSR2_8)) { + goto fail; + } + + // lower retries for run + _dev->set_retries(3); + + _dev->get_semaphore()->give(); + + //register compass instance + _dev->set_device_type(DEVTYPE_QMC5883P); + if (!register_compass(_dev->get_bus_id(), _instance)) { + return false; + } + set_dev_id(_instance, _dev->get_bus_id()); + + printf("%s found on bus %u id %u address 0x%02x\n", name, + _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + + set_rotation(_instance, _rotation); + + if (_force_external) { + set_external(_instance, true); + } + + //Enable 100HZ + _dev->register_periodic_callback(10000, + FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883P::timer, void)); + + return true; + +fail: + _dev->get_semaphore()->give(); + return false; +} +bool AP_Compass_QMC5883P::_check_whoami() +{ + uint8_t whoami; + if (!_dev->read_registers(QMC5883P_REG_ID, &whoami,1)|| + whoami != QMC5883P_ID_VAL) { + return false; + } + return true; +} + +void AP_Compass_QMC5883P::timer() +{ + struct PACKED { + int16_t rx; + int16_t ry; + int16_t rz; + } buffer; + + const float range_scale = 1000.0f / 3000.0f; + + uint8_t status; + if (!_dev->read_registers(QMC5883P_REG_STATUS,&status,1)) { + return; + } + //new data is ready + if (!(status & QMC5883P_STATUS_DATA_READY)) { + return; + } + + if (!_dev->read_registers(QMC5883P_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))) { + return ; + } + + auto x = buffer.rx; + auto y = buffer.ry; + auto z = buffer.rz; + +#if DEBUG + printf("mag.x:%d\n",x); + printf("mag.y:%d\n",y); + printf("mag.z:%d\n",z); +#endif + + Vector3f field = Vector3f{x * range_scale, y * range_scale, z * range_scale }; + + accumulate_sample(field, _instance, 20); +} + +void AP_Compass_QMC5883P::read() +{ + drain_accumulated_samples(_instance); +} + +void AP_Compass_QMC5883P::_dump_registers() +{ + printf("QMC5883P registers dump\n"); + for (uint8_t reg = QMC5883P_REG_DATA_OUTPUT_X; reg <= 0x30; reg++) { + uint8_t v; + _dev->read_registers(reg,&v,1); + printf("%02x:%02x ", (unsigned)reg, (unsigned)v); + if ((reg - ( QMC5883P_REG_DATA_OUTPUT_X-1)) % 16 == 0) { + printf("\n"); + } + } +} diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.h b/libraries/AP_Compass/AP_Compass_QMC5883P.h new file mode 100644 index 0000000000..a6b0d31773 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.h @@ -0,0 +1,68 @@ +/* + * This file 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 file 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 . + * + * Driver by Lokesh Ramina, Jan 2022 + */ +#pragma once + +#include +#include +#include + + +#include "AP_Compass.h" +#include "AP_Compass_Backend.h" + +#ifndef HAL_COMPASS_QMC5883P_I2C_ADDR +#define HAL_COMPASS_QMC5883P_I2C_ADDR 0x2C +#endif + +/* + setup default orientations + */ +#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL +#define HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL ROTATION_ROLL_180 +#endif + +#ifndef HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL +#define HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL ROTATION_ROLL_180_YAW_270 +#endif + +class AP_Compass_QMC5883P : public AP_Compass_Backend +{ +public: + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation); + + void read() override; + + static constexpr const char *name = "QMC5883P"; + +private: + AP_Compass_QMC5883P(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation); + + void _dump_registers(); + bool _check_whoami(); + void timer(); + bool init(); + + AP_HAL::OwnPtr _dev; + + enum Rotation _rotation; + uint8_t _instance; + bool _force_external:1; +}; \ No newline at end of file From dd2502a06576ef7dbf1cccf1a509bb63af7229df Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 1 Jun 2022 21:25:09 +1000 Subject: [PATCH 06/48] Hwdef : CarbonixL496 & CarbonixF405 Add compass QM5883P --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index 1a7a7ca53e..3c567d21e3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -139,7 +139,7 @@ define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 define HAL_PERIPH_ARM_MONITORING_ENABLE 1 BARO MS56XX I2C:0:0x76 -COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE +COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat index 1b5ff08dc7..e4af35fd9c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat @@ -140,7 +140,7 @@ define HAL_PERIPH_ADSB_PORT_DEFAULT 3 define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 BARO MS56XX I2C:0:0x76 -COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE +COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 From 85bf87acefdd0efaef5653f6dd6dab77f803bc56 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 1 Jun 2022 21:24:18 +1000 Subject: [PATCH 07/48] Tools : Decode_devid Add Compass QMC5883P, AK09915 --- Tools/scripts/decode_devid.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py index 15b574306b..b71babe29c 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -64,6 +64,8 @@ def num(s): 0x12 : "DEVTYPE_RM3100", 0x13 : "DEVTYPE_MMC5883", 0x14 : "DEVTYPE_AK09918", + 0x15 : "DEVTYPE_AK09915", + 0x16 : "DEVTYPE_QMC5883P", } imu_types = { From 7b17f874e5ad99c3f0be5bce2f63aec81fdf8c37 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 18 Apr 2023 12:58:44 +1000 Subject: [PATCH 08/48] Hwdef: CarbonixL496 enable arm monitoring --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat index e4af35fd9c..8a85f6468d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat @@ -139,6 +139,8 @@ define HAL_PERIPH_ENABLE_ADSB define HAL_PERIPH_ADSB_PORT_DEFAULT 3 define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 +define HAL_PERIPH_ARM_MONITORING_ENABLE 1 + BARO MS56XX I2C:0:0x76 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 From 3d6138f59141adc419e58e2e95029b299495a3c6 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Thu, 2 Jun 2022 11:55:03 +1000 Subject: [PATCH 09/48] AP_Periph: Print System Init msg with Cersion msg --- Tools/AP_Periph/AP_Periph.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index d5df593d7f..6c0eb99047 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -236,6 +236,8 @@ void AP_Periph_FW::init() #if AP_SCRIPTING_ENABLED scripting.init(); #endif + //custom code carbonix + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CPN Start %s", "AP_Periph V1.3dev_CarboPeriph V4.0"); start_ms = AP_HAL::native_millis(); } From 89b0a34ae0be9c95b2ff8ca0489ea40bb6866491 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 18 Apr 2023 13:03:53 +1000 Subject: [PATCH 10/48] Hwdef: CarbonixL496 GPS Enable with compass and LED --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat index 8a85f6468d..adfe40377a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat @@ -143,7 +143,7 @@ define HAL_PERIPH_ARM_MONITORING_ENABLE 1 BARO MS56XX I2C:0:0x76 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 - +COMPASS IST8310 I2C:0:0x0E false ROTATION_ROLL_180_YAW_90 From 4f9fb0d6cdd677d2a44d20ea34551c7b965f18ff Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Fri, 3 Jun 2022 14:11:55 +1000 Subject: [PATCH 11/48] Hwdef: CarbonixF405 Enable GPS compass and LED --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index 3c567d21e3..3cae8bc10e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -128,7 +128,7 @@ define HAL_WITH_ESC_TELEM 1 # enable GPS define HAL_PERIPH_ENABLE_GPS define HAL_PERIPH_GPS_PORT_DEFAULT 2 -#define HAL_PERIPH_ENABLE_NOTIFY +define HAL_PERIPH_ENABLE_NOTIFY #define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY # default ADSB off by setting 0 baudrate @@ -140,6 +140,7 @@ define HAL_PERIPH_ARM_MONITORING_ENABLE 1 BARO MS56XX I2C:0:0x76 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 +COMPASS IST8310 I2C:0:0x0E false ROTATION_ROLL_180_YAW_90 From 329719553e760dc37fe25f82e7017ad14ae4b4e7 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Sun, 25 Sep 2022 19:19:35 -0700 Subject: [PATCH 12/48] OV3-396 AP_EFI_Hirth : Hirth Driver addition Implementation for Hirth. Base class - AP_EFI WIP: Log functional --- libraries/AP_EFI/AP_EFI.cpp | 201 +++++++++--- libraries/AP_EFI/AP_EFI.h | 27 +- libraries/AP_EFI/AP_EFI_Backend.cpp | 43 +++ libraries/AP_EFI/AP_EFI_Backend.h | 11 +- libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp | 385 +++++++++++++++++++++++ libraries/AP_EFI/AP_EFI_Serial_Hirth.h | 145 +++++++++ libraries/AP_EFI/AP_EFI_State.h | 39 ++- 7 files changed, 810 insertions(+), 41 deletions(-) create mode 100644 libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp create mode 100644 libraries/AP_EFI/AP_EFI_Serial_Hirth.h diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index 3911cd0178..98057ffb19 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -23,7 +23,7 @@ #include "AP_EFI_DroneCAN.h" #include "AP_EFI_Currawong_ECU.h" #include "AP_EFI_Scripting.h" - +#include "AP_EFI_Serial_Hirth.h" #include #if HAL_MAX_CAN_PROTOCOL_DRIVERS @@ -64,6 +64,86 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @User: Advanced AP_GROUPINFO("_FUEL_DENS", 4, AP_EFI, ecu_fuel_density, 0), + // @Param: _THTL_FO + // @DisplayName: Throttle value - First Order + // @Description: First Order Polynomial. (=1, if throttle is first order polynomial trendline) + // @Values: floating values with 0.0001 Resolution. Can be -ve + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_FOP", 5, AP_EFI, throttle_firstorder, 1), + + // @Param: _THTL_SOP + // @DisplayName: Throttle value - Second Order + // @Description: Second Order Polynomial. (=0, if throttle is first order polynomial trendline) + // @Values: floating values with 0.0001 Resolution. Can be -ve + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_SOP", 6, AP_EFI, throttle_secondorder, 0), + + // @Param: _THTL_TOP + // @DisplayName: Throttle value - First Order + // @Description: Third Order Polynomial. (=0, if throttle is first order polynomial trendline) + // @Values: floating values with 0.0001 Resolution. Can be -ve + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_TOP", 7, AP_EFI, throttle_thirdorder, 0), + + // @Param: _THTL_OFF + // @DisplayName: EFI throttle linearization offset + // @Description: Offset for throttle linearization + // @Values: 0 - 100 (0.1 Resolution) + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_OFF", 8, AP_EFI, throttle_offset, 10), + + // @Param: _THROTTLE_IDLE + // @DisplayName: EFI IDLE Throttle value + // @Description: This is the offset value. Ensure ICE_IDLE_PCT=0 for this functionality to work. + // @Values: 0 - 100 (0.1 Resolution) + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_IDLE", 50, AP_EFI, throttle_idle, 15), + + // @Param: _THROTTLE_MAX + // @DisplayName: EFI Max Throttle value + // @Description: Throttle max cap. Throttle scaling is derived from _THROTTLE_MAX and _THROTTLE_IDLE. + // @Values: 0 - 100 (0.1 Resolution) + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_THTL_MAX", 51, AP_EFI, throttle_max, 70), + + // @Param: _EFCR_SLP + // @DisplayName: ECU Fuel Consumption Rate factor + // @Description: ECU FCR gradient/factor. Must be used along with _EFCR_OFT + // @Values: 0 - 1000 (0.1 Resolution) + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_EFCR_SLP", 52, AP_EFI, ecu_fcr_slope, 1), + + // @Param: _EFCR_OFT + // @DisplayName: ECU Fuel Consumption Rate Offset + // @Description: ECU FCR intercept/offset. Must be used along with _EFCR_SLP + // @Values: 0 - 1000 (0.1 Resolution) + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_EFCR_OFT", 53, AP_EFI, ecu_fcr_offset, 0), + + // @Param: _EFCR_AVG + // @DisplayName: ECU Fuel Consumption Rate Average count + // @Description: Averages _EFCR_AVG consecutive reading + // @Values: 0 - 100 (1 Resolution) + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_EFCR_AVG", 54, AP_EFI, ecu_fcr_average_count, 1), + + // @Param: _FUEL_VOL + // @DisplayName: Full Fuel Volume / Capacity + // @Description: Full fuel volume in ml + // @Values: 0 - 65535 (1 Resolution) + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("_FUEL_VOL", 55, AP_EFI, fuel_volume_in_ml, 1), + AP_GROUPEND }; @@ -112,10 +192,15 @@ void AP_EFI::init(void) backend = new AP_EFI_Scripting(*this); #endif break; + case Type::Hirth: + backend = new AP_EFI_Serial_Hirth(*this); + break; default: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unknown EFI type"); break; } + + lua_fuel_consumed = 0; } // Ask all backends to update the frontend @@ -123,6 +208,7 @@ void AP_EFI::update() { if (backend) { backend->update(); + state.estimated_consumed_fuel_volume_cm3 = lua_fuel_consumed; #if HAL_LOGGING_ENABLED log_status(); #endif @@ -173,7 +259,8 @@ void AP_EFI::log_status(void) float(state.oil_pressure), float(state.oil_temperature), float(state.fuel_pressure), - float(state.fuel_consumption_rate_cm3pm), + float(state.fuel_consumption_rate_raw), + //float(state.fuel_consumption_rate_cm3pm), float(state.estimated_consumed_fuel_volume_cm3), uint8_t(state.throttle_position_percent), uint8_t(state.ecu_index)); @@ -194,23 +281,62 @@ void AP_EFI::log_status(void) // @Field: SPU: Spark plug usage // @Field: IDX: Index of the publishing ECU AP::logger().WriteStreaming("EFI2", - "TimeUS,Healthy,ES,GE,CSE,TS,FPS,OPS,DS,MS,DebS,SPU,IDX", - "s------------", - "F------------", - "QBBBBBBBBBBBB", - AP_HAL::micros64(), - uint8_t(is_healthy()), - uint8_t(state.engine_state), - uint8_t(state.general_error), - uint8_t(state.crankshaft_sensor_status), - uint8_t(state.temperature_status), - uint8_t(state.fuel_pressure_status), - uint8_t(state.oil_pressure_status), - uint8_t(state.detonation_status), - uint8_t(state.misfire_status), - uint8_t(state.debris_status), - uint8_t(state.spark_plug_usage), - uint8_t(state.ecu_index)); + "TimeUS,H,ES,SF,ETS,ATS,APS,TS,LCt,CT1_E,CT2_E,FR,FT,FA,IDX", + "s--------------", + "F--------------", + "QBBBBBBBBBBfffB", + AP_HAL::micros64(), + uint8_t(is_healthy()), + uint8_t(state.engine_state), + uint8_t(state.save_in_flash), + uint8_t(state.engine_temperature_sensor_status), + uint8_t(state.air_temperature_sensor_status), + uint8_t(state.air_pressure_sensor_status), + uint8_t(state.throttle_sensor_status), + uint8_t(state.no_of_log_data), + uint8_t(state.CHT_1_error_excess_temperature_status), + uint8_t(state.CHT_2_error_excess_temperature_status), + float(state.fuel_consumption_rate_raw), + float(state.total_fuel_consumed), + float(state.fuel_consumption_rate_average), + uint8_t(state.ecu_index)); + + AP::logger().WriteStreaming("EFI3", + "TimeUS,E1_E,E2_E,C1_T,C2_T,E1_T,E2_T,k_th,thr_f,a_t,e_t,IDX", + "s--OOOOOOOO-", + "F--00000000-", + "QBBffffffffB", + AP_HAL::micros64(), + uint8_t(state.EGT_1_error_excess_temperature_status), + uint8_t(state.EGT_2_error_excess_temperature_status), + float(state.cht1_temp), + float(state.cht2_temp), + float(state.egt1_temp), + float(state.egt2_temp), + float(state.k_throttle), + float(state.thr_pos), + float(state.air_temp), + float(state.eng_temp), + uint8_t(state.ecu_index)); + + AP::logger().WriteStreaming("EFI4", + "TimeUS,BVOL,crc,uptime,lpc,ack,pkt,at,a1,a2,a3,IDX", + "sv----------", + "F-----------", + "QfIIIIIIIIIB", + AP_HAL::micros64(), + float(state.battery_voltage), + uint32_t(state.crc_fail_cnt), + uint32_t(state.uptime), + uint32_t(state.loop_cnt), + uint32_t(state.ack_fail_cnt), + uint32_t(state.packet_sent), + uint32_t(state.ack_thr), + uint32_t(state.ack_s1), + uint32_t(state.ack_s2), + uint32_t(state.ack_s3), + uint8_t(state.ecu_index)); + // @LoggerMessage: ECYL // @Description: EFI per-cylinder information @@ -228,12 +354,12 @@ void AP_EFI::log_status(void) "F-0C0000", "QBfffffB", AP_HAL::micros64(), - 0, - state.cylinder_status.ignition_timing_deg, - state.cylinder_status.injection_time_ms, - state.cylinder_status.cylinder_head_temperature, - state.cylinder_status.exhaust_gas_temperature, - state.cylinder_status.lambda_coefficient, + 0, + state.cylinder_status.ignition_timing_deg, + state.cylinder_status.injection_time_ms, + state.cylinder_status.cylinder_head_temperature, + state.cylinder_status.exhaust_gas_temperature, + state.cylinder_status.lambda_coefficient, state.ecu_index); } #endif // LOGGING_ENABLED @@ -246,6 +372,8 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan) if (!backend) { return; } + // Adding in new requested ECU telemetry fields for Hirth on 18/1/23 for live monitoring + // EGT and CGT variables are already in Celcius from Hirth mavlink_msg_efi_status_send( chan, AP_EFI::is_healthy(), @@ -253,20 +381,20 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan) state.engine_speed_rpm, state.estimated_consumed_fuel_volume_cm3, state.fuel_consumption_rate_cm3pm, - state.engine_load_percent, - state.throttle_position_percent, - state.spark_dwell_time_ms, - state.atmospheric_pressure_kpa, - state.intake_manifold_pressure_kpa, - KELVIN_TO_C(state.intake_manifold_temperature), - KELVIN_TO_C(state.cylinder_status.cylinder_head_temperature), + state.egt2_temp, //was state.engine_load_percent, //EGT2 + state.throttle_position_percent, //throttle position + state.spark_dwell_time_ms, //TBD + state.cht2_temp, //was barometric pressure/state.atmospheric_pressure_kpa, //CHT2 + state.converted_map, //was state.intake_manifold_pressure_kpa + state.air_temp, //was KELVIN_TO_C(state.intake_manifold_temperature), + state.cht1_temp, //KELVIN_TO_C(state.cylinder_status[0].cylinder_head_temperature), //CHT1 state.cylinder_status.ignition_timing_deg, state.cylinder_status.injection_time_ms, - KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature), - state.throttle_out, - state.pt_compensation, + state.egt1_temp, //EGT1 + state.thr_pos, //throttle_out from 0 - 100 + float(state.engine_state),//pt_compensation state.ignition_voltage - ); + ); } // get a copy of state structure @@ -284,4 +412,3 @@ AP_EFI *EFI() } #endif // HAL_EFI_ENABLED - diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index 1fefdd5972..919a45b758 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -58,6 +58,14 @@ class AP_EFI { // Returns the RPM uint32_t get_rpm() const { return state.engine_speed_rpm; } + // for debugging pupose, this value is updated using lua script (which in turn can obtain it from a serial connector) + bool set_fuel_consumed(float fuel_consumed) { + bool status = true; // send true by default. no health check logic implemented + lua_fuel_consumed = fuel_consumed; + // state.estimated_consumed_fuel_volume_cm3 = fuel_consumed; + return status; + } + // returns enabled state of EFI bool enabled() const { return type != Type::NONE; } @@ -84,8 +92,11 @@ class AP_EFI { DroneCAN = 5, CurrawongECU = 6, SCRIPTING = 7, + Hirth = 8, }; + float lua_fuel_consumed; + static AP_EFI *get_singleton(void) { return singleton; } @@ -104,6 +115,20 @@ class AP_EFI { AP_Float coef2; AP_Float ecu_fuel_density; + AP_Float throttle_idle; + AP_Float throttle_max; + + // polynomial config variables for throttle linearization + AP_Float throttle_firstorder; + AP_Float throttle_secondorder; + AP_Float throttle_thirdorder; + AP_Float throttle_offset; + + // ECU fuel consumption calculation + AP_Float ecu_fcr_slope; + AP_Float ecu_fcr_offset; + AP_Int16 ecu_fcr_average_count; + AP_Int16 fuel_volume_in_ml; EFI_State state; @@ -126,4 +151,4 @@ namespace AP { AP_EFI *EFI(); }; -#endif // HAL_EFI_ENABLED +#endif // HAL_EFI_ENABLED \ No newline at end of file diff --git a/libraries/AP_EFI/AP_EFI_Backend.cpp b/libraries/AP_EFI/AP_EFI_Backend.cpp index 8eff55e857..a2de0d94d8 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.cpp +++ b/libraries/AP_EFI/AP_EFI_Backend.cpp @@ -42,6 +42,49 @@ float AP_EFI_Backend::get_coef2(void) const return frontend.coef2; } +float AP_EFI_Backend::get_throttle_firstorder(void) const +{ + return frontend.throttle_firstorder; +} + +float AP_EFI_Backend::get_throttle_secondorder(void) const +{ + return frontend.throttle_secondorder; +} + +float AP_EFI_Backend::get_throttle_thirdorder(void) const +{ + return frontend.throttle_thirdorder; +} + +float AP_EFI_Backend::get_throttle_offset(void) const +{ + return frontend.throttle_offset; +} + +float AP_EFI_Backend::get_ecu_fcr_slope(void) const +{ + return frontend.ecu_fcr_slope; +} + +float AP_EFI_Backend::get_ecu_fcr_offset(void) const +{ + return frontend.ecu_fcr_offset; +} + +int16_t AP_EFI_Backend::get_ecu_fcr_average_count(void) const +{ + if (frontend.ecu_fcr_average_count >= 255){ + return 255; + } + return frontend.ecu_fcr_average_count; +} + +int16_t AP_EFI_Backend::get_fuel_volume_in_ml(void) const +{ + return frontend.fuel_volume_in_ml; +} + HAL_Semaphore &AP_EFI_Backend::get_sem(void) { return frontend.sem; diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h index 9506c7ff09..35ef016342 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -46,7 +46,16 @@ class AP_EFI_Backend { float get_coef1(void) const; float get_coef2(void) const; float get_ecu_fuel_density(void) const; - + float get_throttle_idle(void) const; + float get_throttle_max(void) const; + float get_throttle_firstorder(void) const; + float get_throttle_secondorder(void) const; + float get_throttle_thirdorder(void) const; + float get_throttle_offset(void) const; + float get_ecu_fcr_slope(void) const; + float get_ecu_fcr_offset(void) const; + int16_t get_ecu_fcr_average_count(void) const; + int16_t get_fuel_volume_in_ml(void) const; HAL_Semaphore &get_sem(void); private: diff --git a/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp b/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp new file mode 100644 index 0000000000..d71ee3a22c --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.cpp @@ -0,0 +1,385 @@ +/* + 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_EFI_Serial_Hirth.h" + +#if HAL_EFI_ENABLED +#include +#include +#include + + +extern const AP_HAL::HAL& hal; + +/** + * @brief Constructor with port initialization + * + * @param _frontend + */ +AP_EFI_Serial_Hirth::AP_EFI_Serial_Hirth(AP_EFI &_frontend) : AP_EFI_Backend(_frontend) { + port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0); + get_throttle_polynomial(); + fuel_avg_config = get_ecu_fcr_average_count(); + fuel_avg_count = 0; +} + +/** + * @brief initializes the polynomials + * + */ +void AP_EFI_Serial_Hirth::get_throttle_polynomial() { + throttle_fop = get_throttle_firstorder(); + throttle_sop = get_throttle_secondorder(); + throttle_top = get_throttle_thirdorder(); + throttle_offset = get_throttle_offset(); +} + +/** + * @brief checks for response from or makes requests to Hirth ECU periodically + * + */ +void AP_EFI_Serial_Hirth::update() { + bool status = false; + uint32_t now = AP_HAL::millis(); + + //log the delay in loop interval + internal_state.loop_cnt = now - last_loop_ms; + last_loop_ms = now; + + if ((port != nullptr)) { + + //waiting for response + if (waiting_response) { + + // reset request if not response for SERIAL_WAIT_TIMEOUT-ms + if (now - last_response_ms > SERIAL_WAIT_TIMEOUT) { + waiting_response = false; + last_response_ms = now; + + port->discard_input(); + + internal_state.ack_fail_cnt++; + if(data_send == 1) {internal_state.ack_s1++;} + if(data_send == 2) {internal_state.ack_s2++;} + if(data_send == 3) {internal_state.ack_s3++;} + if(data_send == 5) {internal_state.ack_thr++;} + } + else{ + + num_bytes = port->available(); + + // if already requested + if (num_bytes >= expected_bytes) { + + // read data from buffer + uint8_t computed_checksum = 0; + computed_checksum += res_data.quantity = port->read(); + computed_checksum += res_data.code = port->read(); + + if (res_data.code == req_data.code) { + for (int i = 0; i < (res_data.quantity - QUANTITY_REQUEST_STATUS); i++) { + computed_checksum += raw_data[i] = port->read(); + } + } + + res_data.checksum = port->read(); + computed_checksum = computed_checksum % BYTE_RANGE_MAX; + if (res_data.checksum != (BYTE_RANGE_MAX - computed_checksum)) { + internal_state.crc_fail_cnt++; + port->discard_input(); + } + else { + internal_state.uptime = now - last_uptime; + last_uptime = now; + internal_state.last_updated_ms = now; + decode_data(); + copy_to_frontend(); + } + + waiting_response = false; + } + } + } + + // sending cmd + if(!waiting_response) { + + // get new throttle value + new_throttle = (uint16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); + + // check for change or timeout for throttle value + if ((new_throttle != old_throttle) || (now - last_req_send_throttle > 500)) { + // if new throttle value, send throttle request + // also send new throttle value, only when ARMED + bool allow_throttle = hal.util->get_soft_armed(); + if (!allow_throttle) { + const auto *ice = AP::ice(); + if (ice != nullptr) { + allow_throttle = ice->allow_throttle_disarmed(); + } + } + if (allow_throttle) { + status = send_target_values(new_throttle); + } + else{ + status = send_target_values(0); + } + + old_throttle = new_throttle; + internal_state.k_throttle = new_throttle; + last_req_send_throttle = now; + } + else { + // request Status request, if no throttle commands + status = send_request_status(); + } + + if (status == true) { + waiting_response = true; + last_response_ms = now; + } + } + } +} + + +/** + * @brief updates the current quantity that will be expected + * + */ +void AP_EFI_Serial_Hirth::get_quantity() { + switch (req_data.code) + { + case CODE_REQUEST_STATUS_1: + expected_bytes = QUANTITY_RESPONSE_STATUS_1; + break; + case CODE_REQUEST_STATUS_2: + expected_bytes = QUANTITY_RESPONSE_STATUS_2; + break; + case CODE_REQUEST_STATUS_3: + expected_bytes = QUANTITY_RESPONSE_STATUS_3; + break; + case CODE_SET_VALUE: + expected_bytes = QUANTITY_ACK_SET_VALUES; + break; + } +} + + +/** + * @brief sends the new throttle command to Hirth ECU + * + * @param thr - new throttle value given by SRV_Channel::k_throttle + * @return true - if success + * @return false - currently not implemented + */ +bool AP_EFI_Serial_Hirth::send_target_values(uint16_t thr) { + int idx = 0; + uint8_t computed_checksum = 0; + + // clear buffer + for (size_t i = 0; i < BYTE_RANGE_MAX; i++) + { + raw_data[i] = 0; + } + + // throttle value is computed as a polynomial defined by the config parameters + // to disable linearization, set throttle_top=0, throttle_sop=0, throttle_fop=1, throttle_offset=0 + uint16_t throttle = ((throttle_top * thr * thr * thr) + + (throttle_sop * thr * thr) + + (throttle_fop * thr) + + throttle_offset) * THROTTLE_POSITION_FACTOR; + + // set Quantity + Code + "20 bytes of records to set" + Checksum + computed_checksum += raw_data[idx++] = req_data.quantity = QUANTITY_SET_VALUE; + computed_checksum += raw_data[idx++] = req_data.code = CODE_SET_VALUE; + computed_checksum += raw_data[idx++] = throttle & 0xFF; + computed_checksum += raw_data[idx++] = (throttle >> 0x08) & 0xFF; + + for (; idx < QUANTITY_SET_VALUE - 2; idx++) { + // 0 has no impact on checksum + raw_data[idx] = 0; + } + //checksum calculation + computed_checksum = computed_checksum % BYTE_RANGE_MAX; + raw_data[QUANTITY_SET_VALUE - 1] = (BYTE_RANGE_MAX - computed_checksum); + + //debug + internal_state.packet_sent = 5; + data_send = 5; + + expected_bytes = QUANTITY_ACK_SET_VALUES; + //write data + port->write(raw_data, QUANTITY_SET_VALUE); + + return true; +} + + +/** + * @brief cyclically sends different Status requests to Hirth ECU + * + * @return true - when successful + * @return false - not implemented + */ +bool AP_EFI_Serial_Hirth::send_request_status() { + + bool status = false; + + switch (req_data.code) + { + case CODE_REQUEST_STATUS_1: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_2; + req_data.checksum = CHECKSUM_REQUEST_STATUS_2; + expected_bytes = QUANTITY_RESPONSE_STATUS_2; + internal_state.packet_sent = 2; + data_send =2; + break; + case CODE_REQUEST_STATUS_2: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_3; + req_data.checksum = CHECKSUM_REQUEST_STATUS_3; + expected_bytes = QUANTITY_RESPONSE_STATUS_3; + internal_state.packet_sent = 3; + data_send =3; + break; + case CODE_REQUEST_STATUS_3: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_1; + req_data.checksum = CHECKSUM_REQUEST_STATUS_1; + expected_bytes = QUANTITY_RESPONSE_STATUS_1; + internal_state.packet_sent = 1; + data_send =1; + break; + default: + req_data.quantity = QUANTITY_REQUEST_STATUS; + req_data.code = CODE_REQUEST_STATUS_1; + req_data.checksum = CHECKSUM_REQUEST_STATUS_1; + expected_bytes = QUANTITY_RESPONSE_STATUS_1; + internal_state.packet_sent = 1; + data_send =1; + break; + } + //memcpy(&raw_data, 0, sizeof(raw_data)); + raw_data[0] = req_data.quantity; + raw_data[1] = req_data.code; + raw_data[2] = req_data.checksum; + + port->write(raw_data, QUANTITY_REQUEST_STATUS); + + status = true; + + return status; +} + + +/** + * @brief parses the response from Hirth ECU and updates the internal state instance + * + */ +void AP_EFI_Serial_Hirth::decode_data() { + const uint32_t now_temp = AP_HAL::millis(); + + int engine_status = 0; + + int excess_temp_error = 0; + + switch (res_data.code) + { + case CODE_REQUEST_STATUS_1: + engine_status = (raw_data[8] | raw_data[9] << 0x08); + + //EFI2 Log + internal_state.engine_state = (Engine_State)engine_status; + internal_state.save_in_flash = raw_data[1] | raw_data[2] << 0x08; + internal_state.engine_temperature_sensor_status = raw_data[82] & 0x01; + internal_state.air_temperature_sensor_status = raw_data[82] & 0x02; + internal_state.air_pressure_sensor_status = raw_data[82] & 0x04; + internal_state.throttle_sensor_status = raw_data[82] & 0x08; + internal_state.thr_pos = (raw_data[72] | raw_data[73] << 0x08); + internal_state.air_temp = (raw_data[78] | raw_data[79] << 0x08); + internal_state.eng_temp = (raw_data[74] | raw_data[75] << 0x08); + //add in ADC voltage of MAP sensor > convert to MAP in kPa + internal_state.converted_map = (raw_data[50] | raw_data[51] << 0x08) * ADC_CALIBRATION * MAP_HPA_PER_VOLT_FACTOR * HPA_TO_KPA; + internal_state.battery_voltage = (raw_data[76] | raw_data[77] << 0x08) * VOLTAGE_RESOLUTION; + + //EFI1 Log + internal_state.engine_speed_rpm = (raw_data[10] | raw_data[11] << 0x08); + internal_state.cylinder_status.injection_time_ms = ((raw_data[32] | raw_data[33] << 0x08)) * INJECTION_TIME_RESOLUTION; + internal_state.cylinder_status.ignition_timing_deg = (raw_data[34] | raw_data[35] << 0x08); + //air temperature + internal_state.cylinder_status.cylinder_head_temperature = (raw_data[78] | raw_data[79] << 0x08) + KELVIN_CONVERSION_CONSTANT; + //engine temperature + internal_state.cylinder_status.exhaust_gas_temperature = (raw_data[74] | raw_data[75] << 0x08) + KELVIN_CONVERSION_CONSTANT; + internal_state.crankshaft_sensor_status = ((raw_data[82] & CRANK_SHAFT_SENSOR_OK) == CRANK_SHAFT_SENSOR_OK) ? Crankshaft_Sensor_Status::OK : Crankshaft_Sensor_Status::ERROR; + + internal_state.intake_manifold_temperature = internal_state.air_temp; + + break; + + case CODE_REQUEST_STATUS_2: + + fuel_consumption_rate_raw = (raw_data[52] | raw_data[53] << 0x08) / FUEL_CONSUMPTION_RESOLUTION; + internal_state.fuel_consumption_rate_raw = get_avg_fuel_consumption_rate(fuel_consumption_rate_raw); + internal_state.fuel_consumption_rate_cm3pm = (fuel_consumption_rate_raw * get_ecu_fcr_slope()) + get_ecu_fcr_offset(); + + if (last_fuel_integration_ms != 0) { + internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (now_temp - last_fuel_integration_ms)/60000.0f; + } + last_fuel_integration_ms = now_temp; + + total_fuel_consumed = total_fuel_consumed + internal_state.fuel_consumption_rate_cm3pm; + internal_state.total_fuel_consumed = total_fuel_consumed; + internal_state.throttle_position_percent = (raw_data[62] | raw_data[63] << 0x08) / 10; + break; + + case CODE_REQUEST_STATUS_3: // TBD - internal state addition + excess_temp_error = (raw_data[46] | raw_data[47] << 0x08); + + //EFI2 Log + internal_state.CHT_1_error_excess_temperature_status = (excess_temp_error & 0x01) || (excess_temp_error & 0x02) || (excess_temp_error & 0x04); + internal_state.CHT_2_error_excess_temperature_status = (excess_temp_error & 0x08) || (excess_temp_error & 0x10) || (excess_temp_error & 0x20); + internal_state.EGT_1_error_excess_temperature_status = (excess_temp_error & 0x40) || (excess_temp_error & 0x80) || (excess_temp_error & 0x100); + internal_state.EGT_2_error_excess_temperature_status = (excess_temp_error & 0x200) || (excess_temp_error & 0x400) || (excess_temp_error & 0x800); + internal_state.cht1_temp = (raw_data[16] | raw_data[17] << 0x08); + internal_state.cht2_temp = (raw_data[18] | raw_data[19] << 0x08); + internal_state.egt1_temp = (raw_data[20] | raw_data[21] << 0x08); + internal_state.egt2_temp = (raw_data[22] | raw_data[23] << 0x08); + break; + + // case CODE_SET_VALUE: + // // Do nothing for now + // break; + } +} + + +float AP_EFI_Serial_Hirth::get_avg_fuel_consumption_rate(float fuel_consumed) { + fuel_avg_count = (fuel_avg_count) % fuel_avg_config; + float avg_fuel_consumed = 0; + + instance_fuel_reading[fuel_avg_count++] = fuel_consumed; + for (int i = 0; i < fuel_avg_config; i++) { + avg_fuel_consumed += instance_fuel_reading[i]; + } + + return (avg_fuel_consumed / fuel_avg_config); +} + +#endif // HAL_EFI_ENABLED diff --git a/libraries/AP_EFI/AP_EFI_Serial_Hirth.h b/libraries/AP_EFI/AP_EFI_Serial_Hirth.h new file mode 100644 index 0000000000..55d807e374 --- /dev/null +++ b/libraries/AP_EFI/AP_EFI_Serial_Hirth.h @@ -0,0 +1,145 @@ +/* + 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_EFI.h" +#include "AP_EFI_Backend.h" + + +#define HIRTH_MAX_PKT_SIZE 100 +#define HIRTH_MAX_RAW_PKT_SIZE 103 + +#define BYTE_RANGE_MAX 256 + +#define SERIAL_WAIT_DURATION 100 +#define SERIAL_WAIT_TIMEOUT 100 + +#define ENGINE_RUNNING 4 +#define THROTTLE_POSITION_FACTOR 10 +#define CRANK_SHAFT_SENSOR_OK 0x0F +#define INJECTION_TIME_RESOLUTION 0.8 +#define FUEL_CONSUMPTION_RESOLUTION 10.0 +#define THROTTLE_POSITION_RESOLUTION 0.1 +#define KELVIN_CONVERSION_CONSTANT 273.5 +#define VOLTAGE_RESOLUTION 0.0049 /* 5/1024 */ +#define ADC_CALIBRATION 5.0/1024.0 +#define MAP_HPA_PER_VOLT_FACTOR 248.2673 +#define HPA_TO_KPA 0.1 +#define TPS_SCALE 0.70 + +const uint8_t QUANTITY_REQUEST_STATUS = 0x03; +const uint8_t QUANTITY_SET_VALUE = 0x17; + +const uint8_t CODE_REQUEST_STATUS_1 = 0x04; +const uint8_t CODE_REQUEST_STATUS_2 = 0x0B; +const uint8_t CODE_REQUEST_STATUS_3 = 0x0D; +const uint8_t CODE_SET_VALUE = 0xC9; + +const uint8_t CHECKSUM_REQUEST_STATUS_1 = 0xF9; +const uint8_t CHECKSUM_REQUEST_STATUS_2 = 0xF2; +const uint8_t CHECKSUM_REQUEST_STATUS_3 = 0xF0; +const uint8_t CHECKSUM_SET_VALUE = 0x34; + +const uint8_t QUANTITY_RESPONSE_STATUS_1 = 0x57; +const uint8_t QUANTITY_RESPONSE_STATUS_2 = 0x65; +const uint8_t QUANTITY_RESPONSE_STATUS_3 = 0x67; +const uint8_t QUANTITY_ACK_SET_VALUES = 0x03; + + + +/*! + * Data mapping between rawBytes and Telemetry packets + */ +typedef struct +{ + uint8_t quantity; + uint8_t code; + uint8_t checksum; +} data_set_t; + + +/*! + * class definition for Hirth 4103 ECU + */ +class AP_EFI_Serial_Hirth: public AP_EFI_Backend +{ +public: + AP_EFI_Serial_Hirth(AP_EFI &_frontend); + + void update() override; + + void decode_data(); + + bool send_request_status(); + + bool send_target_values(uint16_t); + + void get_quantity(); + + float get_avg_fuel_consumption_rate(float fuel_consumed); + + void get_throttle_polynomial(); + +private: + // serial port instance + AP_HAL::UARTDriver *port; + + // periodic refresh + uint32_t last_response_ms; + uint32_t last_loop_ms; + uint32_t last_uptime; + uint32_t last_req_send_throttle; + + // raw bytes - max size + uint8_t raw_data[BYTE_RANGE_MAX]; + + // request and response data + data_set_t req_data; + data_set_t res_data; + + // number of bytes + uint32_t num_bytes; + + // TRUE - Request is sent; waiting for response + // FALSE - Response is already received + bool waiting_response; + + // Expected bytes from response + uint8_t expected_bytes; + + // Throttle values + uint16_t new_throttle; + uint16_t old_throttle; + + float fuel_consumption_rate_average; + float fuel_consumption_rate_raw; + float fuel_consumption_rate; + float total_fuel_consumed; + uint32_t last_fuel_integration_ms; + float instance_fuel_reading[BYTE_RANGE_MAX]; + uint8_t fuel_avg_count; + uint8_t fuel_avg_config; + + + float throttle_fop; + float throttle_sop; + float throttle_top; + float throttle_offset; + + uint8_t data_send; +}; diff --git a/libraries/AP_EFI/AP_EFI_State.h b/libraries/AP_EFI/AP_EFI_State.h index 6b0abc93d2..c4f24e1272 100644 --- a/libraries/AP_EFI/AP_EFI_State.h +++ b/libraries/AP_EFI/AP_EFI_State.h @@ -33,8 +33,9 @@ enum class Engine_State : uint8_t { STOPPED = 0, STARTING = 1, - RUNNING = 2, - FAULT = 3 + FAULT = 2, + STARTING_WARM_UP = 3, + RUNNING = 4 }; enum class Crankshaft_Sensor_Status : uint8_t { @@ -206,4 +207,38 @@ struct EFI_State { // PT compensation float pt_compensation; + + //custom parameters CX + uint8_t save_in_flash; + uint8_t engine_temperature_sensor_status; + uint8_t air_temperature_sensor_status; + uint8_t air_pressure_sensor_status; + uint8_t throttle_sensor_status; + uint8_t no_of_log_data; + uint8_t CHT_1_error_excess_temperature_status; + uint8_t CHT_2_error_excess_temperature_status; + uint8_t EGT_1_error_excess_temperature_status; + uint8_t EGT_2_error_excess_temperature_status; + float cht1_temp; + float cht2_temp; + float egt1_temp; + float egt2_temp; + float k_throttle; + float thr_pos; + float air_temp; + float eng_temp; + float battery_voltage; + float converted_map; + uint32_t crc_fail_cnt; + uint32_t uptime; + uint32_t loop_cnt; + uint32_t ack_fail_cnt; + uint32_t packet_sent; + uint32_t ack_thr; + uint32_t ack_s1; + uint32_t ack_s2; + uint32_t ack_s3; + float fuel_consumption_rate_average; + float fuel_consumption_rate_raw; + float total_fuel_consumed; }; From be114d90362b6239829fe6f5dea7878fef455f00 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 18 Apr 2023 15:16:33 +1000 Subject: [PATCH 13/48] OV3-396 AP_SerialManager : Hirth changes For setting BAUD and Buffer size --- libraries/AP_SerialManager/AP_SerialManager.cpp | 2 +- libraries/AP_SerialManager/AP_SerialManager.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 1b04c6d0b5..80a092ef79 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -551,7 +551,7 @@ void AP_SerialManager::init() #endif case SerialProtocol_EFI: - state[i].baud.set_default(AP_SERIALMANAGER_EFI_MS_BAUD); + // state[i].baud.set_default(AP_SERIALMANAGER_EFI_MS_BAUD); uart->begin(state[i].baudrate(), AP_SERIALMANAGER_EFI_MS_BUFSIZE_RX, AP_SERIALMANAGER_EFI_MS_BUFSIZE_TX); diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index c183016e93..794d8ccfd0 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -99,7 +99,7 @@ // MegaSquirt EFI protocol #define AP_SERIALMANAGER_EFI_MS_BAUD 115 #define AP_SERIALMANAGER_EFI_MS_BUFSIZE_RX 512 -#define AP_SERIALMANAGER_EFI_MS_BUFSIZE_TX 16 +#define AP_SERIALMANAGER_EFI_MS_BUFSIZE_TX 32 // SBUS servo outputs #define AP_SERIALMANAGER_SBUS1_BAUD 100000 From 76ff5165b22191ae0c01b3f004a747e3641459a8 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Thu, 20 Apr 2023 14:33:27 +1000 Subject: [PATCH 14/48] OV3-396 AP_ICEngine : add GPIO test code for kill switch includes updates for TCA9554 pin mapping --- libraries/AP_ICEngine/AP_ICEngine.cpp | 108 ++++++++++++++++++++++++++ libraries/AP_ICEngine/AP_ICEngine.h | 42 ++++++++++ 2 files changed, 150 insertions(+) mode change 100644 => 100755 libraries/AP_ICEngine/AP_ICEngine.cpp mode change 100644 => 100755 libraries/AP_ICEngine/AP_ICEngine.h diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp old mode 100644 new mode 100755 index 6e36b3c133..e02d8941e8 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -24,6 +24,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -154,9 +155,22 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Units: RPM AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0), + // @Param: IGNITION_RLY + // @DisplayName: Ignition relay channel + // @Description: This is a a relay channel to use for ignition control + // @User: Standard + // @Values: 0:None, 1:Relay1,2:Relay2,3:Relay3,4:Relay4 + AP_GROUPINFO("IGNITION_RLY", 18, AP_ICEngine, ignition_relay, 0), + AP_GROUPEND }; +#define TCA9554_I2C_BUS 1 +#define TCA9554_I2C_ADDR 0x20 +#define TCA9554_OUTPUT 0x01 // Output Port register address. Outgoing logic levels +#define TCA9554_OUT_DEFAULT 0x30 // 0011 0000 +#define TCA9554_CONF 0x03 // Configuration Port register address [0 = Output] +#define TCA9554_PINS 0xC2 // Set all used ports to outputs = 1100 0010 // constructor AP_ICEngine::AP_ICEngine(const AP_RPM &_rpm) : @@ -182,6 +196,13 @@ void AP_ICEngine::update(void) return; } + if (i2c_state == I2C_UNINITIALIZED) { + i2c_state = I2C_FAILED; + if (TCA9554_init()) { + i2c_state = I2C_SUCCESS; + } + } + uint16_t cvalue = 1500; RC_Channel *c = rc().channel(start_chan-1); if (c != nullptr && rc().has_valid_input()) { @@ -336,6 +357,8 @@ void AP_ICEngine::update(void) case ICE_OFF: SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_off); SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off); + control_ign_str(IGN_OFF_STR_OFF); + ignition_relay_set(false); starter_start_time_ms = 0; break; @@ -343,11 +366,21 @@ void AP_ICEngine::update(void) case ICE_START_DELAY: SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on); SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off); + control_ign_str(IGN_ON_STR_OFF); + ignition_relay_set(false); break; case ICE_STARTING: SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on); SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_on); + if (!(hal.util->get_soft_armed() || allow_throttle_disarmed())) { + control_ign_str(IGN_ON_STR_OFF); + ignition_relay_set(false); + } else { + control_ign_str(IGN_ON_STR_ON_DIR_ON); + ignition_relay_set(true); + } + if (starter_start_time_ms == 0) { starter_start_time_ms = now; } @@ -357,6 +390,8 @@ void AP_ICEngine::update(void) case ICE_RUNNING: SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, pwm_ignition_on); SRV_Channels::set_output_pwm(SRV_Channel::k_starter, pwm_starter_off); + control_ign_str(IGN_ON_STR_OFF); + ignition_relay_set(true); starter_start_time_ms = 0; break; } @@ -537,6 +572,79 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) min_throttle = roundf(idle_governor_integrator); } +bool AP_ICEngine::TCA9554_init() +{ + dev_TCA9554 = std::move(hal.i2c_mgr->get_device(TCA9554_I2C_BUS, TCA9554_I2C_ADDR)); + if (!dev_TCA9554) { + return false; + } + WITH_SEMAPHORE(dev_TCA9554->get_semaphore()); + + dev_TCA9554->set_retries(10); + + // set outputs + bool ret = dev_TCA9554->write_register(TCA9554_OUTPUT, TCA9554_OUT_DEFAULT); + if (!ret) { + return false; + } + ret = dev_TCA9554->write_register(TCA9554_CONF, TCA9554_PINS); + if (!ret) { + return false; + } + TCA9554_set(IGN_OFF_STR_OFF); + dev_TCA9554->set_retries(1); + return true; +} + +void AP_ICEngine::TCA9554_set(TCA9554_state_t value) +{ + if (value != TCA9554_state) { + TCA9554_state = value; + WITH_SEMAPHORE(dev_TCA9554->get_semaphore()); + // set outputs and status leds + //dev_TCA9554->write_register(TCA9554_OUTPUT, (~(value<<2) & 0x0C) | value); + dev_TCA9554->write_register(TCA9554_OUTPUT, (~(value<<2) & 0x0C) | value); + //0011 0010 + //1100 1000 & 0000 1100 + //0000 1000 OR 0011 0010 + //0011 1010 + } +} + +void AP_ICEngine::control_ign_str(TCA9554_state_t value) +{ + if (i2c_state == I2C_SUCCESS) + { + TCA9554_set(value); + } + else + { + //Leave for now + } +} + +/* + control relay for ICE ignition + */ +void AP_ICEngine::ignition_relay_set(bool on) +{ + if (ignition_relay > 0) { + auto *relay = AP::relay(); + if (relay != nullptr) { + if (on) { + relay->on(ignition_relay-1); + } else { + relay->off(ignition_relay-1); + } + } + } +} + +bool AP_ICEngine::allow_throttle_disarmed() const +{ + return option_set(Options::THROTTLE_WHILE_DISARMED) && + hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; +} // singleton instance. Should only ever be set in the constructor. AP_ICEngine *AP_ICEngine::_singleton; diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h old mode 100644 new mode 100755 index f5a1818f22..815587b7b6 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -19,6 +19,10 @@ #pragma once #include "AP_ICEngine_config.h" +#include +#include +#include +#include #if AP_ICENGINE_ENABLED @@ -57,9 +61,44 @@ class AP_ICEngine { static AP_ICEngine *get_singleton() { return _singleton; } + bool allow_throttle_disarmed() const; + private: static AP_ICEngine *_singleton; + /* + * TCA9554 output register mapping for PMB Rev E + * P0 = PMU_EN - PMU output ON/OFF (CN6 pin 2) + * P1 = ECU_EN - Unused (previously Engine Kill Switch) + * P2 = I2C_P2 - Unused + * P3 = LED (active low) + * P4 = PMU_START - Crank Direction (CN6 pin 5) + * P5 = PMU_ARM - Crank Signal (CN6 pin 6) + * P6 = PMU_STAT_IN - Unused + * P7 = PMU_STAT - Unused + */ + + enum TCA9554_state_t { + IGN_OFF_STR_OFF = 0x30, //output register - 0011 0000 + IGN_ON_STR_OFF = 0x30, //output register - 0011 0000 + IGN_ON_STR_ON_DIR_ON = 0x11, //output register - 0001 0001 + } TCA9554_state = IGN_OFF_STR_OFF; + + enum i2c_init_t { + I2C_UNINITIALIZED = 0, + I2C_FAILED = 1, + I2C_SUCCESS = 2 + } i2c_state = I2C_UNINITIALIZED; + + bool TCA9554_init(); + + void TCA9554_set(TCA9554_state_t value); + + void control_ign_str(TCA9554_state_t value); + void ignition_relay_set(bool on); + + AP_HAL::OwnPtr dev_TCA9554; + const class AP_RPM &rpm; enum ICE_State state; @@ -116,6 +155,9 @@ class AP_ICEngine { // Idle Controller Slew Rate AP_Float idle_slew; + // relay number for ignition + AP_Int8 ignition_relay; + // height when we enter ICE_START_HEIGHT_DELAY float initial_height; From 7cfbb9cc64221d9e8dac411c6026a5003ed95ccc Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Thu, 20 Apr 2023 14:39:18 +1000 Subject: [PATCH 15/48] OV3-396 AP_Periph: Addition of ESC APD HVPro Added driver for APD HVPro Telemetry feedback. Will be only compiled if added in hwdef --- Tools/AP_Periph/AP_ESC_APDHVPro.cpp | 110 ++++++++++++++++++++++++++++ Tools/AP_Periph/AP_ESC_APDHVPro.h | 66 +++++++++++++++++ Tools/AP_Periph/AP_Periph.cpp | 4 + Tools/AP_Periph/AP_Periph.h | 6 ++ Tools/AP_Periph/can.cpp | 28 +++++++ 5 files changed, 214 insertions(+) create mode 100644 Tools/AP_Periph/AP_ESC_APDHVPro.cpp create mode 100644 Tools/AP_Periph/AP_ESC_APDHVPro.h diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.cpp b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp new file mode 100644 index 0000000000..47bb3ca7b1 --- /dev/null +++ b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp @@ -0,0 +1,110 @@ + +#include "AP_Periph.h" +#include "AP_ESC_APDHVPro.h" + +#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200 + +extern const AP_HAL::HAL &hal; + +AP_ESC_APDHVPro::AP_ESC_APDHVPro(void){ + +} + +void AP_ESC_APDHVPro::init(AP_HAL::UARTDriver *_uart) { + + port = _uart; + port->begin(57600); + + hal.serial(1)->begin(115200, 128, 256); +} + +bool AP_ESC_APDHVPro::update() { + bool status = false; + + if (!port) { + return status; + } + + uint32_t size = port->available(); + if (size >= ESC_PACKET_SIZE) { + + if (size > ESC_PACKET_SIZE * 2) { + } + else { + int bytes_read = port->read(max_buffer, size); + + if (read_ESC_telemetry_data(bytes_read)) { + last_read_ms = AP_HAL::native_millis(); + + parse_ESC_telemetry_data(); + + status = true; + } + } + } + + return status; +} + +bool AP_ESC_APDHVPro::read_ESC_telemetry_data(uint32_t bytes_read) { + bool status = false; + + if (bytes_read < ESC_PACKET_SIZE) + return status; + + for (int i = 0; i < ESC_PACKET_SIZE; i++) { + raw_buffer[i] = max_buffer[i]; + } + + status = true; + + return status; +} + + +bool AP_ESC_APDHVPro::parse_ESC_telemetry_data() { + bool status = false; + + int check_fletch = check_flectcher16(); + + decoded.voltage = (uint16_t)(((raw_buffer[1] << 8) + raw_buffer[0])/100); + + float rntc = (TEMPERATURE_MAX_RESOLUTION/(float)((raw_buffer[3] << 8) + raw_buffer[2])) - 1; + float temperature1 = (logF((SERIESRESISTOR / rntc) / (float)NOMINAL_RESISTANCE)) / BCOEFFICIENT; + decoded.temperature = (uint16_t)(1 / ((1 / ((float)NOMINAL_RESISTANCE + (float)273.15)) + temperature1)) - (float)273.15; + + decoded.bus_current = (uint16_t)(((raw_buffer[5] << 8) + raw_buffer[4]) / CURRENT_COEFFICIENT); + decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) + raw_buffer[6]); + decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) + (raw_buffer[10] << 16) + (raw_buffer[9] << 8) + raw_buffer[8])) / POLECOUNT; + decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) + raw_buffer[12]) / 10; + decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) + raw_buffer[14]) / 10; + decoded.status = raw_buffer[16]; + decoded.reserved2 = raw_buffer[17]; + decoded.checksum = (uint16_t)((raw_buffer[19] << 8) + raw_buffer[18]); + + // Currently, checksum code is not mandated + if (check_fletch == (int)decoded.checksum) { + status = true; + } + + return status; +} + + +int AP_ESC_APDHVPro::check_flectcher16() { + int fCCRC16; + int i; + int c0 = 0; + int c1 = 0; + + // Calculate checksum intermediate bytesUInt16 + for (i = 0; i < 18; i++) { //Check only first 18 bytes, skip crc bytes + c0 = (int)(c0 + ((int)raw_buffer[i])) % 255; + c1 = (int)(c1 + c0) % 255; + } + // Assemble the 16-bit checksum value + fCCRC16 = ( c1 << 8 ) | c0; + return (int)fCCRC16; +} + +#endif // HAL_PERIPH_ENABLE_ESC_APDHVPRO200 \ No newline at end of file diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.h b/Tools/AP_Periph/AP_ESC_APDHVPro.h new file mode 100644 index 0000000000..e53e48e06d --- /dev/null +++ b/Tools/AP_Periph/AP_ESC_APDHVPro.h @@ -0,0 +1,66 @@ + +#pragma once + +// #include "AP_ESC.h" +#include + +#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200 + +#define SINGLE_LOOP_MAX_BYTES 500 +#define BUFFER_LOOP_SIZE 44 +#define ESC_PACKET_SIZE 22 + +#define POLECOUNT 42 +#define CURRENT_COEFFICIENT 12.5 + +#define SERIESRESISTOR 10000 +#define NOMINAL_RESISTANCE 10000 +#define NOMINAL_TEMPERATURE 25 +#define BCOEFFICIENT 3455 + +#define TEMPERATURE_MAX_RESOLUTION 4096 + + +typedef struct { + uint16_t voltage; + uint16_t temperature; + uint16_t bus_current; + uint16_t reserved1; + uint32_t rpm; + uint16_t input_duty; + uint16_t motor_duty; + uint8_t status; + uint8_t reserved2; + uint16_t checksum; + +}APD_struct; + + +class AP_ESC_APDHVPro { +public: + // constructor + AP_ESC_APDHVPro(); + + void init(AP_HAL::UARTDriver *uart); + + // update the structure + bool update(); + + APD_struct decoded; + +private: + // AP_ESC *_frontend; + + AP_HAL::UARTDriver *port; + + uint32_t last_read_ms; + + uint8_t max_buffer[SINGLE_LOOP_MAX_BYTES]; + uint8_t raw_buffer[ESC_PACKET_SIZE]; + + bool read_ESC_telemetry_data(uint32_t); + bool parse_ESC_telemetry_data(); + int check_flectcher16(); +}; + +#endif // HAL_PERIPH_ENABLE_ESC_APDHVPRO200 \ No newline at end of file diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 6c0eb99047..228b4fc93a 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -223,6 +223,10 @@ void AP_Periph_FW::init() hwesc_telem.init(hal.serial(3)); #endif +#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200 + APD_ESC_Telem.init(hal.serial(1)); +#endif + #ifdef HAL_PERIPH_ENABLE_MSP if (g.msp_port >= 0) { msp_init(hal.serial(g.msp_port)); diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 8e65a8b3f4..5636944f8e 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -21,6 +21,7 @@ #include #include #include +#include "AP_ESC_APDHVPro.h" #if HAL_GCS_ENABLED #include "GCS_MAVLink.h" @@ -206,6 +207,11 @@ class AP_Periph_FW { uint32_t efi_update_ms; #endif +#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200 + AP_ESC_APDHVPro APD_ESC_Telem; + void APD_ESC_Telem_update(); +#endif + #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM AP_ESC_Telem esc_telem; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 0b0745fcf9..2332284491 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1611,6 +1611,30 @@ void AP_Periph_FW::hwesc_telem_update() } #endif // HAL_PERIPH_ENABLE_HWESC +#ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200 +void AP_Periph_FW::APD_ESC_Telem_update() { + if (!APD_ESC_Telem.update()) { + return; + } + + uavcan_equipment_esc_Status pkt {}; + pkt.esc_index = g.esc_number; + pkt.voltage = APD_ESC_Telem.decoded.voltage; + pkt.current = APD_ESC_Telem.decoded.bus_current; + pkt.temperature = APD_ESC_Telem.decoded.temperature; + pkt.rpm = APD_ESC_Telem.decoded.rpm; + // pkt.power_rating_pct = APD_ESC_Telem.decoded. + + uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +} +#endif // HAL_PERIPH_ENABLE_ESC_APDHVPRO200 + #ifdef HAL_PERIPH_ENABLE_RC_OUT #if HAL_WITH_ESC_TELEM /* @@ -1732,7 +1756,11 @@ void AP_Periph_FW::can_update() #ifdef HAL_PERIPH_ENABLE_EFI can_efi_update(); #endif + #ifdef HAL_PERIPH_ENABLE_ESC_APDHVPRO200 + APD_ESC_Telem_update(); + #endif } + const uint32_t now_us = AP_HAL::micros(); while ((AP_HAL::micros() - now_us) < 1000) { hal.scheduler->delay_microseconds(HAL_PERIPH_LOOP_DELAY_US); From 0cfff28d57d0bd4061e47e144969eeea8939e884 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Sun, 25 Sep 2022 22:04:06 -0700 Subject: [PATCH 16/48] OV3-396 hwdef: CarbonixF405 & CarbonixL496 addition of AP ESC HVPro Added AP_ESC_HVPro and also removed ADSB support. --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat | 7 ++++--- libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat | 1 + 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index 3cae8bc10e..9b6e74a6f9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -124,6 +124,7 @@ define HAL_PERIPH_ENABLE_RC_OUT # enable ESC control define HAL_SUPPORT_RCOUT_SERIAL 1 define HAL_WITH_ESC_TELEM 1 +define HAL_PERIPH_ENABLE_ESC_APDHVPRO200 # enable GPS define HAL_PERIPH_ENABLE_GPS @@ -132,9 +133,9 @@ define HAL_PERIPH_ENABLE_NOTIFY #define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY # default ADSB off by setting 0 baudrate -define HAL_PERIPH_ENABLE_ADSB -define HAL_PERIPH_ADSB_PORT_DEFAULT 3 -define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 +#define HAL_PERIPH_ENABLE_ADSB +#define HAL_PERIPH_ADSB_PORT_DEFAULT 3 +#define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 define HAL_PERIPH_ARM_MONITORING_ENABLE 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat index adfe40377a..700c78a3ba 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat @@ -127,6 +127,7 @@ define HAL_PERIPH_ENABLE_RC_OUT # enable ESC control define HAL_SUPPORT_RCOUT_SERIAL 1 define HAL_WITH_ESC_TELEM 1 +define HAL_PERIPH_ENABLE_ESC_APDHVPRO200 # enable GPS define HAL_PERIPH_ENABLE_GPS From a4cdfe15d042415fdd53397c28bc22faa212414f Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Sun, 25 Sep 2022 22:16:25 -0700 Subject: [PATCH 17/48] OV3-396 AP_Periph : parameters addition for APD HVPro ESC --- Tools/AP_Periph/Parameters.cpp | 2 +- Tools/AP_Periph/Parameters.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index e00475fffd..bad110c27f 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -336,7 +336,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100), #endif -#ifdef HAL_PERIPH_ENABLE_HWESC +#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APDHVPRO200) // @Param: ESC_NUMBER // @DisplayName: ESC number // @Description: This is the ESC number to report as in UAVCAN ESC telemetry feedback packets. diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 1d12ccbeae..8d12d4245c 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -99,7 +99,7 @@ class Parameters { AP_Int8 hardpoint_rate; #endif -#ifdef HAL_PERIPH_ENABLE_HWESC +#if defined(HAL_PERIPH_ENABLE_HWESC) || defined(HAL_PERIPH_ENABLE_ESC_APDHVPRO200) AP_Int8 esc_number; #endif From 0b614864c6d421a64c5be7fd8613078c0f14ad0b Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Thu, 20 Apr 2023 14:59:40 +1000 Subject: [PATCH 18/48] OV3-645 AP_Periph: HVPro ESC Squash OV3-396 AP_Perip: AP_ESC_APDHVPro200 : Data type modified OV3-396 AP_ESC_APDHVPro: Current, Voltage and Temperature values are changed from Int to Float OV3-396 AP_ESC_APDHVPro: Add voltage calibration code OV3-522 APDHVPro temporary CURRENT_COEFFICIENT OV3-493 AP_ESC_Telem : revert current coef to 12.5 --- Tools/AP_Periph/AP_ESC_APDHVPro.cpp | 46 +++++++++++++++++++---------- Tools/AP_Periph/AP_ESC_APDHVPro.h | 9 ++++-- Tools/AP_Periph/can.cpp | 2 +- 3 files changed, 38 insertions(+), 19 deletions(-) diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.cpp b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp index 47bb3ca7b1..3dd06ae2c5 100644 --- a/Tools/AP_Periph/AP_ESC_APDHVPro.cpp +++ b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp @@ -36,9 +36,7 @@ bool AP_ESC_APDHVPro::update() { if (read_ESC_telemetry_data(bytes_read)) { last_read_ms = AP_HAL::native_millis(); - parse_ESC_telemetry_data(); - - status = true; + status = parse_ESC_telemetry_data(); } } } @@ -67,20 +65,38 @@ bool AP_ESC_APDHVPro::parse_ESC_telemetry_data() { int check_fletch = check_flectcher16(); - decoded.voltage = (uint16_t)(((raw_buffer[1] << 8) + raw_buffer[0])/100); - - float rntc = (TEMPERATURE_MAX_RESOLUTION/(float)((raw_buffer[3] << 8) + raw_buffer[2])) - 1; - float temperature1 = (logF((SERIESRESISTOR / rntc) / (float)NOMINAL_RESISTANCE)) / BCOEFFICIENT; - decoded.temperature = (uint16_t)(1 / ((1 / ((float)NOMINAL_RESISTANCE + (float)273.15)) + temperature1)) - (float)273.15; - - decoded.bus_current = (uint16_t)(((raw_buffer[5] << 8) + raw_buffer[4]) / CURRENT_COEFFICIENT); - decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) + raw_buffer[6]); - decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) + (raw_buffer[10] << 16) + (raw_buffer[9] << 8) + raw_buffer[8])) / POLECOUNT; - decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) + raw_buffer[12]) / 10; - decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) + raw_buffer[14]) / 10; + //decoded.voltage = (float)(((raw_buffer[1] << 8) | raw_buffer[0])/100.0); + decoded.voltage = (float)(((((raw_buffer[1] << 8) | raw_buffer[0])/100.0) - HVPRO_CALIB_INTCPT)/HVPRO_CALIB_SLOPE); + + float rntc = (TEMPERATURE_MAX_RESOLUTION / (float)((raw_buffer[3] << 8) | raw_buffer[2])) - 1; + + rntc = SERIESRESISTOR / rntc; + + float temperature = rntc / (float)NOMINAL_RESISTANCE; + temperature = (float)logF(temperature); + temperature /= BCOEFFICIENT; + + temperature += (float)1.0 / ((float)NOMINAL_TEMPERATURE + (float)273.15); + temperature = (float)1.0 / temperature; + temperature -= (float)273.15; + + // filter bad values + if (temperature < 0 || temperature > 200){ + temperature = 0; + } + + decoded.temperature = ((float)trunc(temperature * 100) / 100) + 273.15; // Temperature + + float temp_current = (short)((raw_buffer[5] << 8) | raw_buffer[4]); + temp_current /= CURRENT_COEFFICIENT; + decoded.bus_current = (temp_current); + decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) | raw_buffer[6]); + decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) | (raw_buffer[10] << 16) | (raw_buffer[9] << 8) | raw_buffer[8])) / POLEPAIRS; + decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) | raw_buffer[12]) / 10; + decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) | raw_buffer[14]) / 10; decoded.status = raw_buffer[16]; decoded.reserved2 = raw_buffer[17]; - decoded.checksum = (uint16_t)((raw_buffer[19] << 8) + raw_buffer[18]); + decoded.checksum = (uint16_t)((raw_buffer[19] << 8) | raw_buffer[18]); // Currently, checksum code is not mandated if (check_fletch == (int)decoded.checksum) { diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.h b/Tools/AP_Periph/AP_ESC_APDHVPro.h index e53e48e06d..24b6db36d5 100644 --- a/Tools/AP_Periph/AP_ESC_APDHVPro.h +++ b/Tools/AP_Periph/AP_ESC_APDHVPro.h @@ -11,6 +11,7 @@ #define ESC_PACKET_SIZE 22 #define POLECOUNT 42 +#define POLEPAIRS (POLECOUNT / 2) #define CURRENT_COEFFICIENT 12.5 #define SERIESRESISTOR 10000 @@ -20,11 +21,13 @@ #define TEMPERATURE_MAX_RESOLUTION 4096 +#define HVPRO_CALIB_SLOPE 0.9309 +#define HVPRO_CALIB_INTCPT 2.2297 typedef struct { - uint16_t voltage; - uint16_t temperature; - uint16_t bus_current; + float voltage; + float temperature; + float bus_current; uint16_t reserved1; uint32_t rpm; uint16_t input_duty; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 2332284491..0f9471dabb 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1626,7 +1626,7 @@ void AP_Periph_FW::APD_ESC_Telem_update() { // pkt.power_rating_pct = APD_ESC_Telem.decoded. uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, From c3d2ac85d36a0d90cc73300b87541ff3ff7923a9 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Fri, 21 Apr 2023 09:32:12 +1000 Subject: [PATCH 19/48] OV3-398 AP_Periph: Adds a parameter to allow for user configurable timeout ARM Timeout: is changed to 2000 as it is now only scanning ARM signal which is arriving at 1Hz ESC timeout: Timeout defaults to 200ms. Timeout can be disabled by setting parameter to 0. Timeout should be configured to be able to handle at small number of missed packets. --- Tools/AP_Periph/Parameters.cpp | 17 ++++++++++++++++- Tools/AP_Periph/Parameters.h | 2 ++ Tools/AP_Periph/rc_out.cpp | 14 +++++++++++++- 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index bad110c27f..902770976b 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -359,6 +359,14 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @RebootRequired: True GSCALAR(esc_pwm_type, "ESC_PWM_TYPE", 0), + // @Param: ESC_CMD_TIMO + // @DisplayName: ESC Command Timeout + // @Description: This is the duration (ms) with which to hold the last driven ESC command before timing out and zeroing the ESC outputs. To disable zeroing of outputs in event of CAN loss, use 0. Use values greater than the expected duration between two CAN frames to ensure Periph is not starved of ESC Raw Commands. + // @Range: 0 10000 + // @Units: ms + // @User: Advanced + GSCALAR(esc_command_timeout_ms, "ESC_CMD_TIMO", 200), + #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED // @Param: ESC_TELEM_PORT // @DisplayName: ESC Telemetry Serial Port @@ -452,7 +460,14 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #endif #if HAL_PERIPH_ARM_MONITORING_ENABLE - GSCALAR(disarm_delay, "DISARM_DELAY", 1000), + // @Param: DISARM_DELAY + // @DisplayName: ARM Command Timeout + // @Description: This is the duration (ms) with which if not received a signal will disarm the system default to 2 sec as ARM packet is set to 1 Hz + // @Range: 0 10000 + // @Units: ms + // @User: Advanced + GSCALAR(disarm_delay, "DISARM_DELAY", 2000), + #endif AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 8d12d4245c..76347dca60 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -60,6 +60,7 @@ class Parameters { k_param_efi_port, k_param_efi_baudrate, k_param_disarm_delay, + k_param_esc_command_timeout_ms, }; AP_Int16 format_version; @@ -116,6 +117,7 @@ class Parameters { #ifdef HAL_PERIPH_ENABLE_RC_OUT AP_Int8 esc_pwm_type; + AP_Int16 esc_command_timeout_ms; #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED AP_Int8 esc_telem_port; #endif diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 2b105184a6..f260744369 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -131,6 +131,19 @@ void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state) void AP_Periph_FW::rcout_update() { + uint32_t now_ms = AP_HAL::millis(); + + const uint16_t esc_timeout_ms = g.esc_command_timeout_ms >= 0 ? g.esc_command_timeout_ms : 0; // Don't allow negative timeouts! + const bool has_esc_rawcommand_timed_out = esc_timeout_ms != 0 && ((now_ms - last_esc_raw_command_ms) >= esc_timeout_ms); + if (last_esc_num_channels > 0 && has_esc_rawcommand_timed_out) { + // If we've seen ESCs previously, and a timeout has occurred, then zero the outputs + int16_t esc_output[last_esc_num_channels] {}; + rcout_esc(esc_output, last_esc_num_channels); + + // register that the output has been changed + rcout_has_new_data_to_update = true; + } + if (!rcout_has_new_data_to_update) { return; } @@ -141,7 +154,6 @@ void AP_Periph_FW::rcout_update() SRV_Channels::output_ch_all(); SRV_Channels::push(); #if HAL_WITH_ESC_TELEM - uint32_t now_ms = AP_HAL::millis(); if (now_ms - last_esc_telem_update_ms >= 20) { last_esc_telem_update_ms = now_ms; esc_telem_update(); From 39a586b851a46707c53910b3361ede30122afee2 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 7 Dec 2022 14:13:51 +1100 Subject: [PATCH 20/48] OV3-398 AP_Periph: Add timeout to ESC driving when CAN packets are lost ARM _Monitoring: dropped to be also cleared at ESC signal received ESC_Timeout: Fixes a periph continuing to drive an ESC to an output when the esc_rawcommand packets are lost --- Tools/AP_Periph/AP_Periph.cpp | 1 + Tools/AP_Periph/AP_Periph.h | 3 +++ Tools/AP_Periph/can.cpp | 7 ++++--- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 228b4fc93a..ddea6a4ed8 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -393,6 +393,7 @@ void AP_Periph_FW::update() last_error_ms = now; can_printf("IERR 0x%x %u", unsigned(ierr.errors()), unsigned(ierr.last_error_line())); } +//this will only monitor Arming signal #if HAL_PERIPH_ARM_MONITORING_ENABLE static uint32_t last_arm_check_ms; if (now - last_arm_check_ms > g.disarm_delay){ diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 5636944f8e..4f32892211 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -222,6 +222,9 @@ class AP_Periph_FW { SRV_Channels servo_channels; bool rcout_has_new_data_to_update; + uint32_t last_esc_raw_command_ms; + uint8_t last_esc_num_channels; + void rcout_init(); void rcout_init_1Hz(); void rcout_esc(int16_t *rc, uint8_t num_channels); diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 0f9471dabb..90e4ad6c6f 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -765,9 +765,10 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe return; } periph.rcout_esc(cmd.cmd.data, cmd.cmd.len); -#if HAL_PERIPH_ARM_MONITORING_ENABLE - periph.arm_update_status = true; -#endif + + // Update internal copy for disabling output to ESC when CAN packets are lost + periph.last_esc_num_channels = cmd.cmd.len; + periph.last_esc_raw_command_ms = AP_HAL::millis(); } static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) From 546af0ed094cbda2fe9a80b059513bb85437f10f Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Fri, 16 Dec 2022 13:56:43 +1100 Subject: [PATCH 21/48] OV3-416 hwdef: Enable external GPS IS8310 compass --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat | 5 ++++- libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index 9b6e74a6f9..9da06509be 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -141,7 +141,10 @@ define HAL_PERIPH_ARM_MONITORING_ENABLE 1 BARO MS56XX I2C:0:0x76 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 -COMPASS IST8310 I2C:0:0x0E false ROTATION_ROLL_180_YAW_90 + +#GPS Compass Enable +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_NONE diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat index 700c78a3ba..07437488ba 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat @@ -144,7 +144,10 @@ define HAL_PERIPH_ARM_MONITORING_ENABLE 1 BARO MS56XX I2C:0:0x76 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 -COMPASS IST8310 I2C:0:0x0E false ROTATION_ROLL_180_YAW_90 + +#GPS Compass Enable +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_NONE From 375d7fdbf1492a03e77baa31f774f9632818bfaa Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Fri, 21 Apr 2023 09:35:59 +1000 Subject: [PATCH 22/48] OV3-404 AP_Periph: version number in header From now we don't need to edit the print command in AP_Periph .cpp --- Tools/AP_Periph/AP_Periph.cpp | 7 +++++-- Tools/AP_Periph/AP_Periph.h | 1 + Tools/AP_Periph/version.h | 9 ++++++--- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index ddea6a4ed8..a09b4d846c 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -240,8 +240,11 @@ void AP_Periph_FW::init() #if AP_SCRIPTING_ENABLED scripting.init(); #endif - //custom code carbonix - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CPN Start %s", "AP_Periph V1.3dev_CarboPeriph V4.0"); + // custom code carbonix + char *ap_periph_version; + asprintf(&ap_periph_version, "CPN Start (%s)", THISFIRMWARE); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", ap_periph_version); + ap_periph_version = nullptr; start_ms = AP_HAL::native_millis(); } diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 4f32892211..3504bc2daf 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -21,6 +21,7 @@ #include #include #include +#include "version.h" #include "AP_ESC_APDHVPro.h" #if HAL_GCS_ENABLED diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index 4e3f59b496..3a907a7ea6 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -1,16 +1,19 @@ #pragma once #include +#ifndef __AP_PERIPH_FW_VERSION__ +#define __AP_PERIPH_FW_VERSION__ -#define THISFIRMWARE "AP_Periph V1.5dev" +#define THISFIRMWARE "Ottano Carbopilot V4.2.1 OV3-404" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 1,5,0,FIRMWARE_VERSION_TYPE_DEV +#ifndef FW_MAJOR #define FW_MAJOR 1 #define FW_MINOR 5 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV +#endif - - +#endif From 167a92d1d8681cb3acb506bd1c430a2f626e3430 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Fri, 3 Feb 2023 09:06:12 +1100 Subject: [PATCH 23/48] OV3-463 AP_ESC_Telem : logging and putting error_status to backend --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 6 +++++- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 4 +++- libraries/AP_ESC_Telem/LogStructure.h | 4 +++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index b0ee54c5d9..8eab14fc37 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -435,6 +435,9 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) { _telem_data[esc_index].usage_s = new_data.usage_s; } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::ERROR_STATUS) { + _telem_data[esc_index].error_status = new_data.error_status; + } _telem_data[esc_index].count++; _telem_data[esc_index].types |= data_mask; @@ -504,7 +507,8 @@ void AP_ESC_Telem::update() esc_temp : _telem_data[i].temperature_cdeg, current_tot : _telem_data[i].consumption_mah, motor_temp : _telem_data[i].motor_temp_cdeg, - error_rate : _rpm_data[i].error_rate + error_rate : _rpm_data[i].error_rate, + error_status: _telem_data[i].error_status }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); _last_telem_log_ms[i] = _telem_data[i].last_update_ms; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index 769807230f..7bd7610a99 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -25,6 +25,7 @@ class AP_ESC_Telem_Backend { uint32_t last_update_ms; // last update time in milliseconds, determines whether active uint16_t types; // telemetry types present uint16_t count; // number of times updated + uint32_t error_status; // error status }; struct RpmData { @@ -41,7 +42,8 @@ class AP_ESC_Telem_Backend { VOLTAGE = 1 << 2, CURRENT = 1 << 3, CONSUMPTION = 1 << 4, - USAGE = 1 << 5 + USAGE = 1 << 5, + ERROR_STATUS= 1 << 6 }; diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h index 89d0578875..bde745e113 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -17,6 +17,7 @@ // @Field: CTot: current consumed total mAh // @Field: MotTemp: measured motor temperature in centi-degrees C // @Field: Err: error rate +// @Field: error_status: error status struct PACKED log_Esc { LOG_PACKET_HEADER; uint64_t time_us; @@ -29,8 +30,9 @@ struct PACKED log_Esc { float current_tot; int16_t motor_temp; float error_rate; + uint32_t error_status; }; #define LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_ESC_MSG, sizeof(log_Esc), \ - "ESC", "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, + "ESC", "QBeeffcfcfI", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err,ErrS", "s#qqvAOaO%0", "F-BB--BCB--" , true }, From 56615536c5a594ec55609a6f11577c631abc95ac Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 1 Feb 2023 16:27:10 +1100 Subject: [PATCH 24/48] OV3-463 AP_periph: Update ESC status packet error count to error status Have added a new variable "error_status" which is gets its value for HVPro. This value is sent over can "ESC_Status" pkt replacing "error_count" --- Tools/AP_Periph/can.cpp | 1 + Tools/AP_Periph/hwing_esc.cpp | 2 +- Tools/AP_Periph/hwing_esc.h | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 90e4ad6c6f..42e871fdc4 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1624,6 +1624,7 @@ void AP_Periph_FW::APD_ESC_Telem_update() { pkt.current = APD_ESC_Telem.decoded.bus_current; pkt.temperature = APD_ESC_Telem.decoded.temperature; pkt.rpm = APD_ESC_Telem.decoded.rpm; + pkt.error_count = APD_ESC_Telem.decoded.status; // pkt.power_rating_pct = APD_ESC_Telem.decoded. uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; diff --git a/Tools/AP_Periph/hwing_esc.cpp b/Tools/AP_Periph/hwing_esc.cpp index a79862f3cf..9ae4331a77 100644 --- a/Tools/AP_Periph/hwing_esc.cpp +++ b/Tools/AP_Periph/hwing_esc.cpp @@ -135,7 +135,7 @@ bool HWESC_Telem::parse_packet(void) decoded.current = int16_t(be16toh(pkt.current)) * 0.01; decoded.mos_temperature = temperature_decode(pkt.mos_temperature); decoded.cap_temperature = temperature_decode(pkt.cap_temperature); - decoded.status = be16toh(pkt.status); + decoded.status = be32toh(pkt.status); if (decoded.status != 0) { decoded.error_count++; } diff --git a/Tools/AP_Periph/hwing_esc.h b/Tools/AP_Periph/hwing_esc.h index bfb53645f6..dd3d758db6 100644 --- a/Tools/AP_Periph/hwing_esc.h +++ b/Tools/AP_Periph/hwing_esc.h @@ -27,7 +27,7 @@ class HWESC_Telem { float current; uint8_t mos_temperature; uint8_t cap_temperature; - uint16_t status; + uint32_t error_status; uint32_t error_count; }; From 663c96e1e33405b8f5dc289aaf5a3b2f32b07dc7 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 1 Feb 2023 16:11:58 +1100 Subject: [PATCH 25/48] OV3-463 AP_UAVCAN : extract error count from ESC_STATUS pkt extract error_count pkt to error_status --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index baeabe9557..e10c3cf427 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -995,13 +995,15 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E .temperature_cdeg = int16_t((KELVIN_TO_C(cb.msg->temperature)) * 100), .voltage = cb.msg->voltage, .current = cb.msg->current, + .error_status = cb.msg->error_count, }; ap_uavcan->update_rpm(esc_index, cb.msg->rpm); ap_uavcan->update_telem_data(esc_index, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE - | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); + | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE + | AP_ESC_Telem_Backend::TelemetryType::ERROR_STATUS); #endif } From f35974537c57aead2ea373cbf1b291f6cb4a1bad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Jun 2023 18:08:42 +1000 Subject: [PATCH 26/48] AP_UAVCAN: allow BRD_SAFETY_MASK to work on CAN ESCs and servos this allows for testing of a fwd motor or control surfaces while not allowing for VTOL ESCs to run. This makes CAN actuators behave the same as direct PWM actuators --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 39 ++++++++++++++++++++++++------- libraries/AP_UAVCAN/AP_UAVCAN.h | 3 ++- 2 files changed, 33 insertions(+), 9 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index e10c3cf427..fd06b50e70 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -456,10 +456,10 @@ void AP_UAVCAN::loop(void) continue; } - if (_SRV_armed) { + if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) { bool sent_servos = false; - if (_servo_bm > 0) { + if (_SRV_armed_mask != 0) { // if we have any Servos in bitmask uint32_t now = AP_HAL::native_micros(); const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); @@ -474,7 +474,7 @@ void AP_UAVCAN::loop(void) } // if we have any ESC's in bitmask - if (_esc_bm > 0 && !sent_servos) { + if (_ESC_armed_mask != 0 && !sent_servos) { SRV_send_esc(); } @@ -525,7 +525,7 @@ void AP_UAVCAN::SRV_send_actuator(void) * physically possible throws at [-1:1] limits. */ - if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) { + if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _SRV_armed_mask)) { cmd.actuator_id = starting_servo + 1; if (option_is_set(Options::USE_ACTUATOR_PWM)) { @@ -567,7 +567,7 @@ void AP_UAVCAN::SRV_send_esc(void) // find out how many esc we have enabled and if they are active at all for (uint8_t i = esc_offset; i < UAVCAN_SRV_NUMBER; i++) { - if ((((uint32_t) 1) << i) & _esc_bm) { + if ((((uint32_t) 1) << i) & _ESC_armed_mask) { max_esc_num = i + 1; if (_SRV_conf[i].esc_pending) { active_esc_num++; @@ -580,7 +580,7 @@ void AP_UAVCAN::SRV_send_esc(void) k = 0; for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) { - if ((((uint32_t) 1) << i) & _esc_bm) { + if ((((uint32_t) 1) << i) & _ESC_armed_mask) { // TODO: ESC negative scaling for reverse thrust and reverse rotation float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0; @@ -611,7 +611,22 @@ void AP_UAVCAN::SRV_push_servos() } } - _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; + uint16_t servo_armed_mask = _servo_bm; + uint16_t esc_armed_mask = _esc_bm; + const bool safety_off = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; + if (!safety_off) { + AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton(); + if (boardconfig != nullptr) { + const uint16_t safety_mask = boardconfig->get_safety_mask(); + servo_armed_mask &= safety_mask; + esc_armed_mask &= safety_mask; + } else { + servo_armed_mask = 0; + esc_armed_mask = 0; + } + } + _SRV_armed_mask = servo_armed_mask; + _ESC_armed_mask = esc_armed_mask; } @@ -836,7 +851,15 @@ void AP_UAVCAN::safety_state_send() { // handle SafetyState ardupilot::indication::SafetyState safety_msg; - switch (hal.util->safety_switch_state()) { + auto state = hal.util->safety_switch_state(); + if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) { + // if we are outputting any servos or ESCs due to + // BRD_SAFETY_MASK then we need to advertise safety as + // off, this changes LEDs to indicate unsafe and allows + // AP_Periph ESCs and servos to run + state = AP_HAL::Util::SAFETY_ARMED; + } + switch (state) { case AP_HAL::Util::SAFETY_ARMED: safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF; break; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 9085a76c5a..56d0918029 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -287,7 +287,8 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { bool servo_pending; } _SRV_conf[UAVCAN_SRV_NUMBER]; - uint8_t _SRV_armed; + uint32_t _SRV_armed_mask; + uint32_t _ESC_armed_mask; uint32_t _SRV_last_send_us; HAL_Semaphore SRV_sem; From 53c6307614cce7551c0a3f526d944f0515695347 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Jun 2023 19:35:43 +1000 Subject: [PATCH 27/48] AP_ESC_Telem: fixed invalid log structure unit --- libraries/AP_ESC_Telem/LogStructure.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h index bde745e113..91fe179905 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -35,4 +35,4 @@ struct PACKED log_Esc { #define LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_ESC_MSG, sizeof(log_Esc), \ - "ESC", "QBeeffcfcfI", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err,ErrS", "s#qqvAOaO%0", "F-BB--BCB--" , true }, + "ESC", "QBeeffcfcfI", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err,ErrS", "s#qqvAOaO%-", "F-BB--BCB--" , true }, From 3526c42e62f7d52e73d3a9aeab070419df5e7ff3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Jun 2023 19:33:08 +1000 Subject: [PATCH 28/48] Plane: improved fwd throttle during VTOL landing this sets up the vwd integrator more reasonably when we are in POSITION1 stage of VTOL landing. We need to have enough throttle to cope with a headwind, but want it lower when we are at or above our target closing speed so can minimise the amount of pitch up This also makes the landing_desired_closing_velocity() consistent with the landing speed used in approach, using average of airspeed min and cruise speed if TECS_LAND_ARSPD is not set The target airspeed for TECS during airbraking is now set to ARSPD_FBW_MIN, on the basis we are trying to slow down to min speed, and we have VTOL support which should prevent a stall. To cope with a high headwind where ARSPD_FBW_MIN is below the headwind we now check for too low achieved closing speed and switch to POSITION1 which can use vfwd to get to the landing location --- ArduPlane/quadplane.cpp | 35 ++++++++++++++++++++++++++++------- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ebecaaff3d..8a561482bd 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2431,10 +2431,11 @@ void QuadPlane::vtol_position_controller(void) const uint32_t min_airbrake_ms = 1000; if (poscontrol.get_state() == QPOS_AIRBRAKE && poscontrol.time_since_state_start_ms() > min_airbrake_ms && - (aspeed < aspeed_threshold || - fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 || - closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || - labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || + (aspeed < aspeed_threshold || // too low airspeed + fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 || // wrong direction + closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || // too fast + closing_speed < desired_closing_speed*0.5 || // too slow ground speed + labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || // bad attitude labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) { gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 v=%.1f d=%.1f h=%.1f dc=%.1f", (double)groundspeed, @@ -2446,6 +2447,18 @@ void QuadPlane::vtol_position_controller(void) // switch to vfwd for throttle control vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); + + // adjust the initial forward throttle based on our desired and actual closing speed + // this allows for significant initial forward throttle + // when we have a strong headwind, but low throttle in the usual case where + // we want to slow down ready for POSITION2 + vel_forward.integrator = linear_interpolate(0, vel_forward.integrator, + closing_speed, + 1.2*desired_closing_speed, 0.5*desired_closing_speed); + + // limit our initial forward throttle in POSITION1 to be 0.5 of cruise throttle + vel_forward.integrator = constrain_float(vel_forward.integrator, 0, plane.aparm.throttle_cruise*0.5); + vel_forward.last_ms = now_ms; } @@ -3988,6 +4001,10 @@ Vector2f QuadPlane::landing_desired_closing_velocity() float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); if (is_positive(tecs_land_airspeed)) { land_speed = tecs_land_airspeed; + } else { + // use half way between min airspeed and cruise if + // TECS_LAND_AIRSPEED not set + land_speed = 0.5*(land_speed+plane.aparm.airspeed_min); } target_speed = MIN(target_speed, eas2tas * land_speed); @@ -4029,12 +4046,16 @@ float QuadPlane::get_land_airspeed(void) return approach_speed; } + if (qstate == QPOS_AIRBRAKE) { + // during airbraking ask TECS to slow us to stall speed + return plane.aparm.airspeed_min; + } + // calculate speed based on landing desired velocity Vector2f vel = landing_desired_closing_velocity(); - const Vector3f wind = plane.ahrs.wind_estimate(); + const Vector2f wind = plane.ahrs.wind_estimate().xy(); const float eas2tas = plane.ahrs.get_EAS2TAS(); - vel.x -= wind.x; - vel.y -= wind.y; + vel -= wind; vel /= eas2tas; return vel.length(); } From cd44e6a7363bfb23cea710c23e627291a39c89bf Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 27 Jun 2023 16:43:15 -0700 Subject: [PATCH 29/48] OV3-809: AP_Periph: APDHVPRo ESC fixed code as per suggestion 1. port->begin() handled 2. if ( > ESC_PACKET_SIZE * 2) then packets are discarded 3. last_read_ms is updated only if parse is successful 4. divide by '0' is handled 5. proceed only if fletcher checksum succeeds --- Tools/AP_Periph/AP_ESC_APDHVPro.cpp | 74 +++++++++++++++-------------- 1 file changed, 39 insertions(+), 35 deletions(-) diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.cpp b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp index 3dd06ae2c5..709673d1ed 100644 --- a/Tools/AP_Periph/AP_ESC_APDHVPro.cpp +++ b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp @@ -13,9 +13,7 @@ AP_ESC_APDHVPro::AP_ESC_APDHVPro(void){ void AP_ESC_APDHVPro::init(AP_HAL::UARTDriver *_uart) { port = _uart; - port->begin(57600); - - hal.serial(1)->begin(115200, 128, 256); + port->begin(115200, 128, 256); } bool AP_ESC_APDHVPro::update() { @@ -29,14 +27,16 @@ bool AP_ESC_APDHVPro::update() { if (size >= ESC_PACKET_SIZE) { if (size > ESC_PACKET_SIZE * 2) { + port->discard_input(); } else { int bytes_read = port->read(max_buffer, size); if (read_ESC_telemetry_data(bytes_read)) { - last_read_ms = AP_HAL::native_millis(); - status = parse_ESC_telemetry_data(); + if (status) { + last_read_ms = AP_HAL::native_millis(); + } } } } @@ -62,45 +62,49 @@ bool AP_ESC_APDHVPro::read_ESC_telemetry_data(uint32_t bytes_read) { bool AP_ESC_APDHVPro::parse_ESC_telemetry_data() { bool status = false; + float temperature = 0.0; int check_fletch = check_flectcher16(); + decoded.checksum = (uint16_t)((raw_buffer[19] << 8) | raw_buffer[18]); - //decoded.voltage = (float)(((raw_buffer[1] << 8) | raw_buffer[0])/100.0); - decoded.voltage = (float)(((((raw_buffer[1] << 8) | raw_buffer[0])/100.0) - HVPRO_CALIB_INTCPT)/HVPRO_CALIB_SLOPE); - - float rntc = (TEMPERATURE_MAX_RESOLUTION / (float)((raw_buffer[3] << 8) | raw_buffer[2])) - 1; + // Currently, checksum code is not mandated + if (check_fletch == (int)decoded.checksum) { + status = true; - rntc = SERIESRESISTOR / rntc; + decoded.voltage = (float)(((((raw_buffer[1] << 8) | raw_buffer[0]) / 100.0) - HVPRO_CALIB_INTCPT) / HVPRO_CALIB_SLOPE); - float temperature = rntc / (float)NOMINAL_RESISTANCE; - temperature = (float)logF(temperature); - temperature /= BCOEFFICIENT; + float temp = (float)((raw_buffer[3] << 8) | raw_buffer[2]); + if (temp > 0) + { + float rntc = (TEMPERATURE_MAX_RESOLUTION / temp) - 1; - temperature += (float)1.0 / ((float)NOMINAL_TEMPERATURE + (float)273.15); - temperature = (float)1.0 / temperature; - temperature -= (float)273.15; + rntc = SERIESRESISTOR / rntc; - // filter bad values - if (temperature < 0 || temperature > 200){ - temperature = 0; - } + temperature = rntc / (float)NOMINAL_RESISTANCE; + temperature = (float)logF(temperature); + temperature /= BCOEFFICIENT; - decoded.temperature = ((float)trunc(temperature * 100) / 100) + 273.15; // Temperature - - float temp_current = (short)((raw_buffer[5] << 8) | raw_buffer[4]); - temp_current /= CURRENT_COEFFICIENT; - decoded.bus_current = (temp_current); - decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) | raw_buffer[6]); - decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) | (raw_buffer[10] << 16) | (raw_buffer[9] << 8) | raw_buffer[8])) / POLEPAIRS; - decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) | raw_buffer[12]) / 10; - decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) | raw_buffer[14]) / 10; - decoded.status = raw_buffer[16]; - decoded.reserved2 = raw_buffer[17]; - decoded.checksum = (uint16_t)((raw_buffer[19] << 8) | raw_buffer[18]); + temperature += (float)1.0 / ((float)NOMINAL_TEMPERATURE + (float)273.15); + temperature = (temperature > 0) ? (float)1.0 / temperature : 0; + temperature -= (float)273.15; + } - // Currently, checksum code is not mandated - if (check_fletch == (int)decoded.checksum) { - status = true; + // filter bad values + if (temperature < 0 || temperature > 200) { + temperature = 0; + } + + decoded.temperature = ((float)trunc(temperature * 100) / 100) + 273.15; // Temperature + + float temp_current = (short)((raw_buffer[5] << 8) | raw_buffer[4]); + temp_current /= CURRENT_COEFFICIENT; + decoded.bus_current = (temp_current); + decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) | raw_buffer[6]); + decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) | (raw_buffer[10] << 16) | (raw_buffer[9] << 8) | raw_buffer[8])) / POLEPAIRS; + decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) | raw_buffer[12]) / 10; + decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) | raw_buffer[14]) / 10; + decoded.status = raw_buffer[16]; + decoded.reserved2 = raw_buffer[17]; } return status; From 99e504cf225d39724c45e03fe820eb8a60eaa8ba Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Sun, 16 Jul 2023 17:44:23 -0700 Subject: [PATCH 30/48] OV3-400 AP_UAVCAN: CAN Node status Log Implemented CANH log section which includes - Health, Uptime and Delta of CAN devices Update libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h Co-authored-by: Jono453 <32658975+Jono453@users.noreply.github.com> --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 2 +- libraries/AP_UAVCAN/AP_UAVCAN.h | 1 + libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp | 33 ++++++++++++++++++++ libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h | 3 ++ 4 files changed, 38 insertions(+), 1 deletion(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index fd06b50e70..d1ff4cfe24 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -123,7 +123,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @Param: OPTION // @DisplayName: UAVCAN options // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,5:LogNodeStatus // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 56d0918029..f346da35b4 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -204,6 +204,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { CANFD_ENABLED = (1U<<2), DNA_IGNORE_UNHEALTHY_NODE = (1U<<3), USE_ACTUATOR_PWM = (1U<<4), + LOG_NODE_STATUS = (1U<<5), }; // check if a option is set diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp index 0e1b50cd03..1b0efa8134 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp @@ -488,6 +488,16 @@ void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb server_state = HEALTHY; } } + + //to calculate the delta b/w handlenode status pkt + uint32_t now = AP_HAL::native_millis(); + + if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::LOG_NODE_STATUS)) { + log_NodeStatus(node_id, cb.msg->uptime_sec, cb.msg->health, cb.msg->mode , now - last_can_init_delta_ms[node_id]); + } + + last_can_init_delta_ms[node_id] = now; + if (!isNodeIDVerified(node_id)) { //immediately begin verification of the node_id if (getNodeInfo_client[driver_index] != nullptr) { @@ -580,6 +590,29 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], } } +/* + optionally log NodeStatus packets + */ +void AP_UAVCAN_DNA_Server::log_NodeStatus(uint8_t node_id, uint32_t uptime_sec, uint8_t healthy, uint8_t mode, uint32_t delta) +{ + if (node_id > MAX_NODE_ID) { + return; + } + + // @LoggerMessage: CANH + // @Description: CAN Health Status + // @Field: TimeUS: Time since system startup + // @Field: NodeId: Node ID + // @Field: Healthy: zero when node healthy + // @Field: UpTime: time since boot in seconds + AP::logger().WriteStreaming("CANH", + "TimeUS," "NodeID," "Healthy," "UpTime," "delta", // labels + "s" "#" "-" "-" "-", // units + "F" "-" "-" "-" "-", // multipliers + "Q" "B" "B" "I" "I", // types + AP_HAL::micros64(), node_id, healthy, uptime_sec, delta); +} + //Trampoline call for handleNodeInfo member call void AP_UAVCAN_DNA_Server::trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp) { diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h index 3a6d6ae14f..33de748f88 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h @@ -132,6 +132,9 @@ class AP_UAVCAN_DNA_Server //Run through the list of seen node ids for verification void verify_nodes(); + // Log NodeInfo + void log_NodeStatus(uint8_t node_id, uint32_t uptime_sec, uint8_t healthy, uint8_t mode, uint32_t delta); + uint32_t last_can_init_delta_ms[126]; }; #endif From f9d034b29d8fa67008c4a28ef4460588abb4fd12 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Sun, 16 Jul 2023 17:56:48 -0700 Subject: [PATCH 31/48] OV3-733: AP_Bootloader: reserve 100 IDs for Carbonix boards and allocating new Ids --- Tools/AP_Bootloader/board_types.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 87864143e1..b31562f5b8 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -194,3 +194,15 @@ AP_HW_SWBOOMBOARD_PERIPH 1403 AP_HW_CubeOrange_ODID 10140 AP_HW_Pixhawk6_ODID 10053 + +# IDs 5000-5100 reserved for Carbonix +AP_HW_CarbonixCubeOrange 5000 +AP_HW_CarbonixF405_M1 5001 +AP_HW_CarbonixF405_M2 5002 +AP_HW_CarbonixF405_M3 5003 +AP_HW_CarbonixF405_M4 5004 +AP_HW_CarbonixF405_M5 5005 +AP_HW_CarbonixF405_LWing 5006 +AP_HW_CarbonixF405_RWing 5007 +AP_HW_CarbonixF405_LTail 5008 +AP_HW_CarbonixF405_RTail 5009 \ No newline at end of file From 93787b98e0fd01e70bc74a6bac589e92ff196f84 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Mon, 10 Jul 2023 19:04:43 -0700 Subject: [PATCH 32/48] OV3-733: Tools: Carbonix waf build script --- Tools/Carbonix_scripts/carbonix_waf_build.sh | 97 ++++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100755 Tools/Carbonix_scripts/carbonix_waf_build.sh diff --git a/Tools/Carbonix_scripts/carbonix_waf_build.sh b/Tools/Carbonix_scripts/carbonix_waf_build.sh new file mode 100755 index 0000000000..1c68adee41 --- /dev/null +++ b/Tools/Carbonix_scripts/carbonix_waf_build.sh @@ -0,0 +1,97 @@ +#!/bin/bash + + +# Check unstaged changes +if [[ $(git status --porcelain) ]]; then + echo "There are unstaged changes in the repository." + exit 1 +fi + +# Check outdated submodules +# git submodule update --remote --quiet +# outdated_submodules=$(git submodule status | grep -e '-[[:alnum:]]' | awk '{print $2}') +# if [[ -n $outdated_submodules ]]; then +# echo "The following submodules are not updated:" +# echo "$outdated_submodules" +# exit 1 +# fi + +echo "Running distclean..." +./waf distclean + +# ArduPlane with CubeOrange +echo "Compiling ArduPlane for CarbonixCubeOrange..." +./waf configure --board CarbonixCubeOrange +./waf plane +./waf configure --board CarbonixCubeOrange --bootloader +./waf bootloader + +echo "Compiling ArduPlane for CubeOrange..." +./waf configure --board CubeOrange +./waf plane +./waf configure --board CubeOrange --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405..." +./waf configure --board CarbonixF405 +./waf AP_Periph +./waf configure --board CarbonixF405 --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_M1..." +./waf configure --board CarbonixF405-M1 +./waf AP_Periph +./waf configure --board CarbonixF405-M1 --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_M2..." +./waf configure --board CarbonixF405-M2 +./waf AP_Periph +./waf configure --board CarbonixF405-M2 --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_M3..." +./waf configure --board CarbonixF405-M3 +./waf AP_Periph +./waf configure --board CarbonixF405-M3 --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_M4..." +./waf configure --board CarbonixF405-M4 +./waf AP_Periph +./waf configure --board CarbonixF405-M4 --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_M5..." +./waf configure --board CarbonixF405-M5 +./waf AP_Periph +./waf configure --board CarbonixF405-M5 --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_LWing..." +./waf configure --board CarbonixF405-LWing +./waf AP_Periph +./waf configure --board CarbonixF405-LWing --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_RWing..." +./waf configure --board CarbonixF405-RWing +./waf AP_Periph +./waf configure --board CarbonixF405-RWing --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_RTail..." +./waf configure --board CarbonixF405-RTail +./waf AP_Periph +./waf configure --board CarbonixF405-RTail --bootloader +./waf bootloader + +echo "Compiling AP_Periph for CarbonixF405_RTail..." +./waf configure --board CarbonixF405-LTail +./waf AP_Periph +./waf configure --board CarbonixF405-LTail --bootloader +./waf bootloader + +# echo "Compiling SITL for CarbonixF405..." +# ./waf configure --board sitl +# ./Tools/autotest/sim_vehicle.py --console -f quadplane --vehicle=ArduPlane \ No newline at end of file From 270fe3286cb3574eb01e5fede647a0571d87df0f Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Mon, 10 Jul 2023 19:06:22 -0700 Subject: [PATCH 33/48] OV3-733: hwdef: add new boards Carbonix --- .../hwdef/CarbonixCubeOrange/defaults.parm | 0 .../hwdef/CarbonixCubeOrange/hwdef-bl.dat | 16 ++++++++++++++++ .../hwdef/CarbonixCubeOrange/hwdef.dat | 15 +++++++++++++++ .../hwdef/CarbonixF405-LTail/defaults.parm | 0 .../hwdef/CarbonixF405-LTail/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-LTail/hwdef.dat | 9 +++++++++ .../hwdef/CarbonixF405-LWing/defaults.parm | 0 .../hwdef/CarbonixF405-LWing/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-LWing/hwdef.dat | 9 +++++++++ .../hwdef/CarbonixF405-M1/defaults.parm | 0 .../hwdef/CarbonixF405-M1/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-M1/hwdef.dat | 9 +++++++++ .../hwdef/CarbonixF405-M2/defaults.parm | 0 .../hwdef/CarbonixF405-M2/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-M2/hwdef.dat | 9 +++++++++ .../hwdef/CarbonixF405-M3/defaults.parm | 0 .../hwdef/CarbonixF405-M3/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-M3/hwdef.dat | 8 ++++++++ .../hwdef/CarbonixF405-M4/defaults.parm | 0 .../hwdef/CarbonixF405-M4/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-M4/hwdef.dat | 8 ++++++++ .../hwdef/CarbonixF405-M5/defaults.parm | 0 .../hwdef/CarbonixF405-M5/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-M5/hwdef.dat | 8 ++++++++ .../hwdef/CarbonixF405-RTail/defaults.parm | 0 .../hwdef/CarbonixF405-RTail/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-RTail/hwdef.dat | 9 +++++++++ .../hwdef/CarbonixF405-RWing/defaults.parm | 0 .../hwdef/CarbonixF405-RWing/hwdef-bl.dat | 8 ++++++++ .../hwdef/CarbonixF405-RWing/hwdef.dat | 9 +++++++++ 30 files changed, 181 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef-bl.dat new file mode 100644 index 0000000000..06557e5b42 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef-bl.dat @@ -0,0 +1,16 @@ +# hw definition file for processing by chibios_hwdef.py +# for H743 bootloader + +include ../CubeOrange/hwdef-bl.dat + +undef USB_STRING_PRODUCT +undef USB_STRING_MANUFACTURER +undef APJ_BOARD_ID +undef USB_PRODUCT + +# USB setup +USB_PRODUCT 0x1056 +USB_STRING_MANUFACTURER "CubePilot" +USB_STRING_PRODUCT "Carbonix-bl" + +APJ_BOARD_ID 5000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat new file mode 100644 index 0000000000..c86431b5f4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/hwdef.dat @@ -0,0 +1,15 @@ +# hw definition file for processing by chibios_hwdef.py + +include ../CubeOrange/hwdef.dat + +undef USB_STRING_PRODUCT +undef USB_STRING_MANUFACTURER +undef APJ_BOARD_ID +undef USB_PRODUCT + +# USB setup +USB_PRODUCT 0x1057 +USB_STRING_MANUFACTURER "CubePilot" +USB_STRING_PRODUCT "Carbonix" + +APJ_BOARD_ID 5000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef-bl.dat new file mode 100644 index 0000000000..35b51d805b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5008 + +define CAN_APP_NODE_NAME "CPN-27-Left-Tail" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef.dat new file mode 100644 index 0000000000..b5d05539f0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LTail/hwdef.dat @@ -0,0 +1,9 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5008 + +define CAN_APP_NODE_NAME "CPN-27-Left-Tail" + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef-bl.dat new file mode 100644 index 0000000000..aca49879e6 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5006 + +define CAN_APP_NODE_NAME "CPN-26-Left-Wing" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef.dat new file mode 100644 index 0000000000..1381083d32 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-LWing/hwdef.dat @@ -0,0 +1,9 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5006 + +define CAN_APP_NODE_NAME "CPN-26-Left-Wing" + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef-bl.dat new file mode 100644 index 0000000000..b932184550 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5001 + +define CAN_APP_NODE_NAME "CPN-11-Motor-1" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef.dat new file mode 100644 index 0000000000..975f7179fa --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M1/hwdef.dat @@ -0,0 +1,9 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5001 + +define CAN_APP_NODE_NAME "CPN-11-Motor-1" + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef-bl.dat new file mode 100644 index 0000000000..ae045b604b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5002 + +define CAN_APP_NODE_NAME "CPN-12-Motor-2" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef.dat new file mode 100644 index 0000000000..dcc67fc75a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M2/hwdef.dat @@ -0,0 +1,9 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5002 + +define CAN_APP_NODE_NAME "CPN-12-Motor-2" + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef-bl.dat new file mode 100644 index 0000000000..e9432facd3 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5003 + +define CAN_APP_NODE_NAME "CPN-23-Motor-3" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef.dat new file mode 100644 index 0000000000..d118a02233 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M3/hwdef.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5003 + +define CAN_APP_NODE_NAME "CPN-23-Motor-3" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef-bl.dat new file mode 100644 index 0000000000..1f195d2039 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5004 + +define CAN_APP_NODE_NAME "CPN-24-Motor-4" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef.dat new file mode 100644 index 0000000000..3fb1b9d2d8 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M4/hwdef.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5004 + +define CAN_APP_NODE_NAME "CPN-24-Motor-4" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef-bl.dat new file mode 100644 index 0000000000..024b0630bc --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5005 + +define CAN_APP_NODE_NAME "CPN-15-Pusher" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef.dat new file mode 100644 index 0000000000..1e35c45bcb --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-M5/hwdef.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5005 + +define CAN_APP_NODE_NAME "CPN-15-Pusher" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef-bl.dat new file mode 100644 index 0000000000..eae4bf756c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5009 + +define CAN_APP_NODE_NAME "CPN-17-Right-Tail" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef.dat new file mode 100644 index 0000000000..35138a6454 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RTail/hwdef.dat @@ -0,0 +1,9 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5009 + +define CAN_APP_NODE_NAME "CPN-17-Right-Tail" + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/defaults.parm new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef-bl.dat new file mode 100644 index 0000000000..28fafd40d3 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef-bl.dat @@ -0,0 +1,8 @@ +include ../CarbonixF405/hwdef-bl.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5007 + +define CAN_APP_NODE_NAME "CPN-16-Right-Wing" \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef.dat new file mode 100644 index 0000000000..a12fc21f2c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405-RWing/hwdef.dat @@ -0,0 +1,9 @@ +include ../CarbonixF405/hwdef.dat + +undef CAN_APP_NODE_NAME +undef APJ_BOARD_ID + +APJ_BOARD_ID 5007 + +define CAN_APP_NODE_NAME "CPN-16-Right-Wing" + From e1a28ee9c27b36023f8eedcd8fde5ea2643faefd Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Mon, 10 Jul 2023 19:08:48 -0700 Subject: [PATCH 34/48] OV3-733: Script add to CubeOrange Cx_LED_Driver --- .../scripts/CX_LED_driver.lua | 50 +++++++++++++++++++ .../CubeOrange/scripts/CX_LED_driver.lua | 50 +++++++++++++++++++ 2 files changed, 100 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/CX_LED_driver.lua create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/CX_LED_driver.lua diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/CX_LED_driver.lua b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/CX_LED_driver.lua new file mode 100644 index 0000000000..c532fa1bf9 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixCubeOrange/scripts/CX_LED_driver.lua @@ -0,0 +1,50 @@ + +-- LED is switched on using motor function +local MOTOR_LED_FUNCTION = 94 + +-- Ensure that the out2_function min = 200 and max = 10000 +local MOTOR_LED_PWM_VAL_TRIM = 5000 +local MOTOR_LED_PWM_VAL_MIN = 200 +local MOTOR_LED_PWM_VAL_MAX = 10000 +local MOTOR_LED_PWM_VAL_OFF = 0 + +-- Arming check interval +local LED_CYCLE_TIME = 2000 + + +local LED_servo = SRV_Channels:find_channel(MOTOR_LED_FUNCTION) + + +gcs:send_text(0, "LED -- BEGIN") + + +-- LED is switched ON using a PWM signal configured as MOTOR_LED_FUNCTION +-- This is done to overcome the design limitations on CPN +local function switch_LED(LED_val) + + if (LED_servo ~= null) then + SRV_Channels:set_output_pwm_chan_timeout(LED_servo, LED_val, LED_CYCLE_TIME) + end + + return check_arming, LED_CYCLE_TIME +end + + + +-- Function to continuously check if the vehicle is armed or not +-- ARMED -> LED_ON +-- DISARMED -> LED_OFF +local function check_arming() + vehicle_armed = arming:is_armed() + + if vehicle_armed == true then + switch_LED(MOTOR_LED_PWM_VAL_MAX) + else + switch_LED(MOTOR_LED_PWM_VAL_MIN) + end + + return check_arming, LED_CYCLE_TIME +end + +return check_arming(), 10000 + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/CX_LED_driver.lua b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/CX_LED_driver.lua new file mode 100644 index 0000000000..c532fa1bf9 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/scripts/CX_LED_driver.lua @@ -0,0 +1,50 @@ + +-- LED is switched on using motor function +local MOTOR_LED_FUNCTION = 94 + +-- Ensure that the out2_function min = 200 and max = 10000 +local MOTOR_LED_PWM_VAL_TRIM = 5000 +local MOTOR_LED_PWM_VAL_MIN = 200 +local MOTOR_LED_PWM_VAL_MAX = 10000 +local MOTOR_LED_PWM_VAL_OFF = 0 + +-- Arming check interval +local LED_CYCLE_TIME = 2000 + + +local LED_servo = SRV_Channels:find_channel(MOTOR_LED_FUNCTION) + + +gcs:send_text(0, "LED -- BEGIN") + + +-- LED is switched ON using a PWM signal configured as MOTOR_LED_FUNCTION +-- This is done to overcome the design limitations on CPN +local function switch_LED(LED_val) + + if (LED_servo ~= null) then + SRV_Channels:set_output_pwm_chan_timeout(LED_servo, LED_val, LED_CYCLE_TIME) + end + + return check_arming, LED_CYCLE_TIME +end + + + +-- Function to continuously check if the vehicle is armed or not +-- ARMED -> LED_ON +-- DISARMED -> LED_OFF +local function check_arming() + vehicle_armed = arming:is_armed() + + if vehicle_armed == true then + switch_LED(MOTOR_LED_PWM_VAL_MAX) + else + switch_LED(MOTOR_LED_PWM_VAL_MIN) + end + + return check_arming, LED_CYCLE_TIME +end + +return check_arming(), 10000 + From 1d9669fd2b85d4fa726e158f12333463409b6c61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 7 Dec 2022 13:35:15 +1100 Subject: [PATCH 35/48] VO1-147 AP_HAL: added statistics structure for CAN Cherry picked from OV3-415 --- libraries/AP_HAL/CANIface.h | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index 30a5564e3e..cd17c42b91 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -220,9 +220,30 @@ class AP_HAL::CANIface return 0; } + typedef struct { + uint32_t tx_requests; + uint32_t tx_rejected; + uint32_t tx_overflow; + uint32_t tx_success; + uint32_t tx_timedout; + uint32_t tx_abort; + uint32_t rx_received; + uint32_t rx_overflow; + uint32_t rx_errors; + uint32_t num_busoff_err; + } bus_stats_t; + +#if !defined(HAL_BOOTLOADER_BUILD) //Get status info of the interface virtual void get_stats(ExpandingString &str) {} + /* + return bus statistics for logging + return nullptr if no statistics available + */ + virtual const bus_stats_t *get_statistics(void) const { return nullptr; }; +#endif + // return true if busoff was detected and not cleared virtual bool is_busoff() const { From d5797fd274adba6cef0c2f188e99ff13e388af68 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Fri, 5 May 2023 10:23:52 +1000 Subject: [PATCH 36/48] OV3-691 hwdef: Add crystal 12Mhz , RangeFinder --- libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat | 4 ++-- libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat index c9b65a069f..e048f428f4 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat @@ -15,8 +15,8 @@ APJ_BOARD_ID 1064 # setup build for a peripheral firmware env AP_PERIPH 1 -# crystal frequency set to 0 to use internal clock -OSCILLATOR_HZ 0 +# crystal frequency set to 0 to use internal clock; currently configured for external crystal 12MHZ +OSCILLATOR_HZ 12000000 # assume 1024K flash part FLASH_SIZE_KB 1024 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index 9da06509be..3b5649331a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -16,8 +16,8 @@ APJ_BOARD_ID 1064 # setup build for a peripheral firmware env AP_PERIPH 1 -# crystal frequency set to 0 to use internal clock -OSCILLATOR_HZ 0 +# crystal frequency set to 12MHz to use internal clock +OSCILLATOR_HZ 12000000 #MCU F405 Flash 1024 FLASH_SIZE_KB 1024 @@ -146,7 +146,8 @@ COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 define HAL_PROBE_EXTERNAL_I2C_COMPASSES COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_NONE - +# enable Rangefinder +define HAL_PERIPH_ENABLE_RANGEFINDER 1 From 342391c30200767efbc2c0aa780343bd41415693 Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Sun, 16 Jul 2023 19:16:22 -0700 Subject: [PATCH 37/48] VO1-147 AP_UAVCAN: log bus statistics Cherry picked From: OV3-415 --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 61 ++++++++++++++++++++++++++++++- libraries/AP_UAVCAN/AP_UAVCAN.h | 11 ++++++ 2 files changed, 70 insertions(+), 2 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index d1ff4cfe24..a6970eafe1 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -494,6 +494,7 @@ void AP_UAVCAN::loop(void) #if AP_OPENDRONEID_ENABLED AP::opendroneid().dronecan_send(this); #endif + logging(); } } @@ -543,7 +544,11 @@ void AP_UAVCAN::SRV_send_actuator(void) } if (i > 0) { - act_out_array[_driver_index]->broadcast(msg); + if (act_out_array[_driver_index]->broadcast(msg) > 0) { + _srv_send_count++; + } else { + _fail_send_count++; + } if (i == 15) { repeat_send = true; @@ -594,7 +599,11 @@ void AP_UAVCAN::SRV_send_esc(void) k++; } - esc_raw[_driver_index]->broadcast(esc_msg); + if (esc_raw[_driver_index]->broadcast(esc_msg) > 0) { + _esc_send_count++; + } else { + _fail_send_count++; + } } } @@ -1242,4 +1251,52 @@ bool AP_UAVCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const return _dna_server->prearm_check(fail_msg, fail_msg_len); } +/* + periodic logging + */ +void AP_UAVCAN::logging(void) +{ +#if HAL_LOGGING_ENABLED + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_log_ms < 1000) { + return; + } + last_log_ms = now_ms; + if (HAL_NUM_CAN_IFACES <= _driver_index) { + // no interface? + return; + } + const auto *iface = hal.can[_driver_index]; + if (iface == nullptr) { + return; + } + const auto *stats = iface->get_statistics(); + if (stats == nullptr) { + // statistics not implemented on this interface + return; + } + const auto &s = *stats; + AP::logger().WriteStreaming("CANS", + "TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx", + "s#-------------", + "F--------------", + "QBIIIIIIIIIIIII", + AP_HAL::micros64(), + _driver_index, + s.tx_success, + s.tx_requests, + s.tx_rejected, + s.tx_overflow, + s.tx_timedout, + s.tx_abort, + s.rx_received, + s.rx_overflow, + s.rx_errors, + s.num_busoff_err, + _esc_send_count, + _srv_send_count, + _fail_send_count); +#endif // HAL_LOGGING_ENABLED +} + #endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index f346da35b4..919e6fb3ae 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -248,6 +248,9 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { // send parameter save request void send_parameter_save_request(); + // periodic logging + void logging(); + // set parameter on a node ParamGetSetIntCb *param_int_cb; ParamGetSetFloatCb *param_float_cb; @@ -290,9 +293,17 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { uint32_t _SRV_armed_mask; uint32_t _ESC_armed_mask; + uint32_t _esc_send_count; + uint32_t _srv_send_count; + uint32_t _fail_send_count; + + uint8_t _SRV_armed; uint32_t _SRV_last_send_us; HAL_Semaphore SRV_sem; + // last log time + uint32_t last_log_ms; + ///// LED ///// struct led_device { uint8_t led_index; From 516c12b92a583308a4bd8b7ad1431caed20983a2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 27 Jun 2023 00:02:15 +1000 Subject: [PATCH 38/48] AP_Math: add sum_of_bytes_in_buffer_mod_256 just sums the values of the bytes in a buffer and returns that sum modulus 256 --- libraries/AP_Math/crc.cpp | 9 +++++++++ libraries/AP_Math/crc.h | 4 ++++ 2 files changed, 13 insertions(+) diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 9027477166..f1dc6e91a4 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -494,3 +494,12 @@ uint64_t crc_crc64(const uint32_t *data, uint16_t num_words) return crc; } + +uint8_t sum_of_bytes_in_buffer_mod_256(uint8_t *data, uint16_t count) +{ + uint8_t ret = 0; + for (uint8_t i=0; i Date: Sun, 16 Jul 2023 19:35:32 -0700 Subject: [PATCH 39/48] OV3-645: MS EFI update --- libraries/SITL/SIM_EFI_MegaSquirt.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.cpp b/libraries/SITL/SIM_EFI_MegaSquirt.cpp index 7aa29ff229..e6b2ae94a8 100644 --- a/libraries/SITL/SIM_EFI_MegaSquirt.cpp +++ b/libraries/SITL/SIM_EFI_MegaSquirt.cpp @@ -20,6 +20,7 @@ #include #include #include +#include "SIM_EFI_MegaSquirt.h" using namespace SITL; @@ -37,7 +38,7 @@ static uint32_t CRC32_MS(const uint8_t *buf, uint32_t len) void EFI_MegaSquirt::update() { auto sitl = AP::sitl(); - if (!sitl || sitl->efi_type == SIM::EFI_TYPE_NONE) { + if (!sitl || sitl->efi_type == SIM::EFI_TYPE_MS) { return; } float rpm = sitl->state.rpm[0]; From 37c4e4f8a3e9a5c12ed21a36f1a8d18c55cb933c Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Sun, 16 Jul 2023 19:38:24 -0700 Subject: [PATCH 40/48] OV3-645: Hirth SITL integration wip --- libraries/AP_HAL_SITL/SITL_State.cpp | 10 +- libraries/AP_HAL_SITL/SITL_State.h | 4 + libraries/SITL/SIM_EFI_HIrth.cpp | 166 +++++++++++++++++++++++++++ libraries/SITL/SIM_EFI_Hirth.h | 140 ++++++++++++++++++++++ libraries/SITL/SITL.h | 1 + 5 files changed, 320 insertions(+), 1 deletion(-) create mode 100644 libraries/SITL/SIM_EFI_HIrth.cpp create mode 100644 libraries/SITL/SIM_EFI_Hirth.h diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index ca03aaf360..4039681b0e 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -382,6 +382,12 @@ SITL::SerialDevice *SITL_State::create_serial_sim(const char *name, const char * } efi_ms = new SITL::EFI_MegaSquirt(); return efi_ms; + } else if (streq(name, "hirth")) { + if (efi_hirth != nullptr) { + AP_HAL::panic("Only one hirth at a time"); + } + efi_hirth = new SITL::EFI_Hirth(); + return efi_hirth; } else if (streq(name, "VectorNav")) { if (vectornav != nullptr) { AP_HAL::panic("Only one VectorNav at a time"); @@ -622,7 +628,9 @@ void SITL_State::_fdm_input_local(void) if (efi_ms != nullptr) { efi_ms->update(); } - + if (efi_hirth != nullptr) { + efi_hirth->update(); + } if (frsky_d != nullptr) { frsky_d->update(); } diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 872c33f460..b4bf4c399a 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -287,6 +288,9 @@ class HALSITL::SITL_State { // simulated EFI MegaSquirt device: SITL::EFI_MegaSquirt *efi_ms; + // simulated EFI Hirth device: + SITL::EFI_Hirth *efi_hirth; + // output socket for flightgear viewing SocketAPM fg_socket{true}; diff --git a/libraries/SITL/SIM_EFI_HIrth.cpp b/libraries/SITL/SIM_EFI_HIrth.cpp new file mode 100644 index 0000000000..7977143b75 --- /dev/null +++ b/libraries/SITL/SIM_EFI_HIrth.cpp @@ -0,0 +1,166 @@ +/* + 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 . + */ +/* + simulate Hirth EFI system +*/ + +#include "SIM_Aircraft.h" +#include +#include +#include +#include "SIM_EFI_Hirth.h" + +using namespace SITL; + +void EFI_Hirth::update_receive() +{ + const ssize_t num_bytes_read = read_from_autopilot((char*)&receive_buf[receive_buf_ofs], ARRAY_SIZE(receive_buf) - receive_buf_ofs); + if (num_bytes_read < 1) { + return; + } + receive_buf_ofs += num_bytes_read; + + if (receive_buf_ofs < 1) { + return; + } + + const uint8_t expected_bytes_in_message = receive_buf[0]; + + if (expected_bytes_in_message == 0) { + AP_HAL::panic("zero bytes expected is unexpected"); + } + + if (expected_bytes_in_message > ARRAY_SIZE(receive_buf)) { + AP_HAL::panic("Unexpectedly large byte count"); + } + + if (receive_buf_ofs < expected_bytes_in_message) { + return; + } + + // checksum is sum of all bytes except the received checksum: + const uint8_t expected_checksum = 256 - sum_of_bytes_in_buffer_mod_256(receive_buf, expected_bytes_in_message-1); + const uint8_t received_checksum = receive_buf[expected_bytes_in_message-1]; + if (expected_checksum == received_checksum) { + PacketCode received_packet_code = PacketCode(receive_buf[1]); + if (received_packet_code == PacketCode::SetValues) { + // do this synchronously for now + handle_set_values(); + } else { + assert_receive_size(3); + if (requested_data_record.time_ms != 0) { + AP_HAL::panic("Requesting too fast?"); + } + requested_data_record.code = received_packet_code; + requested_data_record.time_ms = AP_HAL::millis(); + } + } else { + AP_HAL::panic("checksum failed"); + // simply throw these bytes away. What the actual device does in the + // face of weird data is unknown. + } + memmove(&receive_buf[0], &receive_buf[expected_bytes_in_message], receive_buf_ofs - expected_bytes_in_message); + receive_buf_ofs -= expected_bytes_in_message; +} + +void EFI_Hirth::assert_receive_size(uint8_t receive_size) +{ + if (receive_buf[0] != receive_size) { + AP_HAL::panic("Expected %u message size, got %u message size", receive_size, receive_buf[0]); + } +} + +void EFI_Hirth::handle_set_values() +{ + assert_receive_size(23); + static_assert(sizeof(settings) == 20, "correct number of bytes in settings"); + memcpy((void*)&settings, &receive_buf[2], sizeof(settings)); +} + +void EFI_Hirth::update_send() +{ + if (requested_data_record.time_ms == 0) { + // no outstanding request + return; + } + if (AP_HAL::millis() - requested_data_record.time_ms < 20) { + // 20ms to service a request + return; + } + requested_data_record.time_ms = 0; + + switch (requested_data_record.code) { + case PacketCode::DataRecord1: + send_record1(); + break; + case PacketCode::DataRecord2: + send_record2(); + break; + case PacketCode::DataRecord3: + send_record3(); + break; + default: + AP_HAL::panic("Unknown data record (%u) requested", (unsigned)requested_data_record.code); + } +} + +void EFI_Hirth::update() +{ + auto sitl = AP::sitl(); + if (!sitl || sitl->efi_type != SIM::EFI_TYPE_HIRTH) { + return; + } + + // update throttle; interim thing to make life a little more interesting + // gcs().send_text(MAV_SEVERITY_INFO, "using throttle %f", settings.throttle/10.0); + throttle = 0.9 * throttle + 0.1 * (settings.throttle)/10.0; + + update_receive(); + update_send(); +} + +void SITL::EFI_Hirth::send_record1() +{ + const auto *sitl = AP::sitl(); + + auto &r = packed_record1.record; + + r.rpm = sitl->state.rpm[0]; + gcs().send_text(MAV_SEVERITY_INFO, "rpm in: %u", r.rpm); + r.air_temperature = AP::baro().get_temperature(); + + packed_record1.update_checksum(); + + static_assert(sizeof(record1) == 84, "correct number of bytes in record1"); + write_to_autopilot((char*)&packed_record1, sizeof(packed_record1)); +} + +void SITL::EFI_Hirth::send_record2() +{ + packed_record2.record.throttle = uint16_t(throttle * 10.0); + packed_record2.update_checksum(); + + static_assert(sizeof(record2) == 98, "correct number of bytes in record2"); + write_to_autopilot((char*)&packed_record2, sizeof(packed_record2)); +} + + +void SITL::EFI_Hirth::send_record3() +{ + packed_record3.update_checksum(); + + static_assert(sizeof(record3) == 100, "correct number of bytes in record3"); + write_to_autopilot((char*)&packed_record3, sizeof(packed_record3)); +} \ No newline at end of file diff --git a/libraries/SITL/SIM_EFI_Hirth.h b/libraries/SITL/SIM_EFI_Hirth.h new file mode 100644 index 0000000000..02dc658543 --- /dev/null +++ b/libraries/SITL/SIM_EFI_Hirth.h @@ -0,0 +1,140 @@ +/* + 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 . + */ +/* + simulate Hirth EFI system + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:hirth --speedup=1 +param set SERIAL5_PROTOCOL 24 +param set SIM_EFI_TYPE 6 +param set EFI_TYPE 6 +reboot +status EFI_STATUS + +./Tools/autotest/autotest.py --gdb --debug build.Plane test.Plane.Hirth + +*/ + +#pragma once + +#include +#include +#include "SIM_SerialDevice.h" + +namespace SITL { + +class EFI_Hirth : public SerialDevice { +public: + + using SerialDevice::SerialDevice; + + void update(); + +private: + + void update_receive(); + void update_send(); + + void assert_receive_size(uint8_t receive_size); + + void handle_set_values(); + + // maps from an on-wire number to a record number: + enum class PacketCode : uint8_t { + DataRecord1 = 4, + DataRecord2 = 11, + DataRecord3 = 13, + SetValues = 201, + }; + + template + class PACKED PackedRecord { + public: + PackedRecord(PacketCode _code, T _record) : + code(uint8_t(_code)), + record(_record) + { } + const uint8_t length { sizeof(T) + 3 }; // 1 each of length, code and checksum + const uint8_t code; + T record; + uint8_t checksum; + + void update() { + record.update(); + update_checksum(); + } + + void update_checksum() { + checksum = 256 - sum_of_bytes_in_buffer_mod_256((uint8_t*)this, length-1); + } + }; + + void send_record1(); + void send_record2(); + void send_record3(); + + class PACKED Record1 { + public: + uint8_t unknown1[10]; + uint16_t rpm; + uint8_t unknown2[66]; + uint16_t air_temperature; + uint8_t unknown3[4]; + + void update(); + }; + + class PACKED Record2 { + public: + uint8_t unknown1[62]; + uint16_t throttle; // percent * 10 + uint8_t unknown2[34]; + }; + + class PACKED Record3 { + public: + uint8_t unknown1[100]; + }; + + class PACKED SetValues { + public: + uint16_t throttle; // percent * 10 + uint8_t unknown1[18]; + }; + + // these records are just used for initial values of the fields; + // they aren't used past that. + Record1 record1; + Record2 record2; + Record3 record3; + + + SetValues settings; + + PackedRecord packed_record1{PacketCode::DataRecord1, record1}; + PackedRecord packed_record2{PacketCode::DataRecord2, record2}; + PackedRecord packed_record3{PacketCode::DataRecord3, record3}; + + struct { + PacketCode code; // code which was requested by driver + uint32_t time_ms; // time that code was requested by driver + } requested_data_record; + + uint8_t receive_buf[32]; + uint8_t receive_buf_ofs; + + float throttle; +}; + +} \ No newline at end of file diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index b2a39a3f87..0250252908 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -275,6 +275,7 @@ class SIM { enum EFIType { EFI_TYPE_NONE = 0, EFI_TYPE_MS = 1, + EFI_TYPE_HIRTH = 8, }; AP_Int8 efi_type; From 2546675fa0ea206a34ee1d130061982c871a78a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Jul 2023 15:42:18 +1000 Subject: [PATCH 41/48] Tools: added SpringValley3 location --- Tools/autotest/locations.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index 541a099634..7b00117b03 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -56,6 +56,7 @@ Elvenes=68.871422,17.986690,17,256 Kawachi=35.879129,140.339683,7,0 SpringValley=-35.280252,149.005821,597.3,5 SpringValley2=-35.28240059,149.00542037,582,10 +SpringValley3=-35.28240515,149.00716878,579,12.6 Pyramid=29.9764,31.1339,0,0 AAMEastField=39.842288,-105.212928,1809,106 HachinoheMine=40.4539496,141.5419051,56,270 From 64484258c23f6446fd2ddb2a625eb4546e5b92fd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Jul 2023 09:30:36 +1000 Subject: [PATCH 42/48] Ottano SITL setup --- ArduPlane/Ottano/eeprom-base.bin | Bin 0 -> 16384 bytes ArduPlane/Ottano/go.sh | 8 ++++++++ ArduPlane/Ottano/test/mavinit.scr | 5 +++++ 3 files changed, 13 insertions(+) create mode 100644 ArduPlane/Ottano/eeprom-base.bin create mode 100755 ArduPlane/Ottano/go.sh create mode 100644 ArduPlane/Ottano/test/mavinit.scr diff --git a/ArduPlane/Ottano/eeprom-base.bin b/ArduPlane/Ottano/eeprom-base.bin new file mode 100644 index 0000000000000000000000000000000000000000..0f3ef9b00724db4a22b4de3ad9e526f32708055a GIT binary patch literal 16384 zcmeI!&r1|x7zgn0JM-$Uy1Q&@CLW5_ERi}W6dpod$ME1qDq$d`{Lw8;=wej1Ka?(M zCJ`MQ@M!)Bf;|+tYqyYAFCB`c2oJ9vG|Jt@1$$;D_fHU_XAaLZ&pR{E^S&Qg*q(4P zpaFges{&GX?CACICPUcQXtf?);VNK4FN1zhV8Wn0MmbLT0p*o4*w=*a`d8*wG@l#?Mi&!Z zlbssAEF`)-$rDl+v$bem*z1%=6RPeu1U99gUXHX$BZSpaV3-aeG2G$Am=jY@)P%F{ z#F!IP?uth1Pw1-vfz&|=YY@fHsh;y+rhRDAI06-*7SU~8$kJh}C!i{q58?woR>NGzgE3HU%*gr>-<~Gr6FP4Z44ZEoq;0I=n?k zeVqDzVRgJm^o8=RMg25Tidl#v3w97FF0wS}cj&W|7^Qd=o9{Mspvn-|D`>*{Nqe7N zcX4PhqPq?l+qvB$Q_<=(qI<}m6luFD(|MZMzQ5mQFYv_z|Chl04IoHIKKBj&-mH4K zm^L@)Cx&12b9S#QR3^$$el@FhU#3lZSp-jGu{Eb^z8A{C`JCFFT0409=0is1Zxza# zPZ{+&kv5kA9nA-g@z!m1%F3#z&F;$$b$B$NRU<8KIdTm7cusBi70Tn0oT~ol&YkmP zw=t)#zfYUJ_MU;|OIbCwlr{?^_=W{ofCX591z3OuSm2NawrJk9_MxnTZ_ON#FZOZ@ zVfusiBc~!~*Ue>bMn!__CVDdpfwVbz$lK#_EWiRRzyd750xZA+EWiRRzyd750xZA+ zEWiRRzyd750xZA+EWiRRzyd750xZA+EWiRRzyd750xZA+EWiRRzyd750{?)(A6Cl9 A>Hq)$ literal 0 HcmV?d00001 diff --git a/ArduPlane/Ottano/go.sh b/ArduPlane/Ottano/go.sh new file mode 100755 index 0000000000..bbdddf097a --- /dev/null +++ b/ArduPlane/Ottano/go.sh @@ -0,0 +1,8 @@ +#!/bin/bash + +[ -f eeprom.bin ] || { + echo "Creating eeprom.bin" + cp eeprom-base.bin eeprom.bin +} + +nice ../../Tools/autotest/sim_vehicle.py -D -G -f quadplane-ice --console --map -L SpringValley3 --aircraft test "$*" -A --uartF=sim:megasquirt diff --git a/ArduPlane/Ottano/test/mavinit.scr b/ArduPlane/Ottano/test/mavinit.scr new file mode 100644 index 0000000000..dd42b4a07c --- /dev/null +++ b/ArduPlane/Ottano/test/mavinit.scr @@ -0,0 +1,5 @@ +console add RPM "RPM: %.0f" RPM.rpm1 +console add vrpm "VRPM: %.0f %.0f %.0f %.0f" ESC_TELEMETRY_1_TO_4.rpm[0],ESC_TELEMETRY_1_TO_4.rpm[1],ESC_TELEMETRY_1_TO_4.rpm[2],ESC_TELEMETRY_1_TO_4.rpm[3] +console add CHead "CHead: %.0f %.0f" EFI_STATUS.cylinder_head_temperature,EFI_STATUS.barometric_pressure +console add Ign "Ign: %.0f" RC_CHANNELS.chan7_raw +console add Aspd "Aspd %.1f %.1f" VFR_HUD.airspeed,NAMED_VALUE_FLOAT["AS2"].value 5 From 2b10d13d36e02a2d8867a1f380ecc3fa6de2ad1f Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Tue, 18 Jul 2023 16:13:55 -0700 Subject: [PATCH 43/48] OV3-645 : AP_UAVCAN: Logn Node status changed to ENABLE_STATS LOG_NODE_STATUS changed to ENABLE_STATS which is bit 8 --- libraries/AP_UAVCAN/AP_UAVCAN.h | 2 +- libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 919e6fb3ae..fd8aceb6e3 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -204,7 +204,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { CANFD_ENABLED = (1U<<2), DNA_IGNORE_UNHEALTHY_NODE = (1U<<3), USE_ACTUATOR_PWM = (1U<<4), - LOG_NODE_STATUS = (1U<<5), + ENABLE_STATS = (1U<<8), }; // check if a option is set diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp index 1b0efa8134..3c97876011 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp @@ -492,7 +492,7 @@ void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb //to calculate the delta b/w handlenode status pkt uint32_t now = AP_HAL::native_millis(); - if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::LOG_NODE_STATUS)) { + if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::ENABLE_STATS)) { log_NodeStatus(node_id, cb.msg->uptime_sec, cb.msg->health, cb.msg->mode , now - last_can_init_delta_ms[node_id]); } From 77b018d39bbf664d05e7482bdcc18c5a92e84a22 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 17 Jul 2023 15:25:53 -0700 Subject: [PATCH 44/48] Plane: On vtol landings if from a mission perform crosstracking --- ArduPlane/quadplane.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 8a561482bd..2b050518e6 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2364,7 +2364,7 @@ void QuadPlane::vtol_position_controller(void) const float aspeed_threshold = MAX(plane.aparm.airspeed_min-2, assist_speed); // run fixed wing navigation - plane.nav_controller->update_waypoint(plane.current_loc, loc); + plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, loc); // use TECS for throttle SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.TECS_controller.get_throttle_demand()); @@ -3157,7 +3157,8 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) plane.crash_state.is_crashed = false; // also update nav_controller for status output - plane.nav_controller->update_waypoint(plane.current_loc, plane.next_WP_loc); + plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, + plane.next_WP_loc); poscontrol_init_approach(); return true; From 3f39bb251769b556d275ae094b874110f8e0f8bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Aug 2023 12:55:01 +1000 Subject: [PATCH 45/48] AP_EFI: fixed parameter metadata for Hirth parameters --- libraries/AP_EFI/AP_EFI.cpp | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index 98057ffb19..15d95698ab 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -67,7 +67,7 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _THTL_FO // @DisplayName: Throttle value - First Order // @Description: First Order Polynomial. (=1, if throttle is first order polynomial trendline) - // @Values: floating values with 0.0001 Resolution. Can be -ve + // @Range: -1 1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_THTL_FOP", 5, AP_EFI, throttle_firstorder, 1), @@ -75,7 +75,7 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _THTL_SOP // @DisplayName: Throttle value - Second Order // @Description: Second Order Polynomial. (=0, if throttle is first order polynomial trendline) - // @Values: floating values with 0.0001 Resolution. Can be -ve + // @Range: -1 1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_THTL_SOP", 6, AP_EFI, throttle_secondorder, 0), @@ -83,7 +83,7 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _THTL_TOP // @DisplayName: Throttle value - First Order // @Description: Third Order Polynomial. (=0, if throttle is first order polynomial trendline) - // @Values: floating values with 0.0001 Resolution. Can be -ve + // @Range: -1 1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_THTL_TOP", 7, AP_EFI, throttle_thirdorder, 0), @@ -91,7 +91,7 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _THTL_OFF // @DisplayName: EFI throttle linearization offset // @Description: Offset for throttle linearization - // @Values: 0 - 100 (0.1 Resolution) + // @Range: 0 100 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_THTL_OFF", 8, AP_EFI, throttle_offset, 10), @@ -99,7 +99,7 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _THROTTLE_IDLE // @DisplayName: EFI IDLE Throttle value // @Description: This is the offset value. Ensure ICE_IDLE_PCT=0 for this functionality to work. - // @Values: 0 - 100 (0.1 Resolution) + // @Range: 0 100 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_THTL_IDLE", 50, AP_EFI, throttle_idle, 15), @@ -107,7 +107,8 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _THROTTLE_MAX // @DisplayName: EFI Max Throttle value // @Description: Throttle max cap. Throttle scaling is derived from _THROTTLE_MAX and _THROTTLE_IDLE. - // @Values: 0 - 100 (0.1 Resolution) + // @Range: 0 100 + // @Increment: 0.1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_THTL_MAX", 51, AP_EFI, throttle_max, 70), @@ -115,7 +116,8 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _EFCR_SLP // @DisplayName: ECU Fuel Consumption Rate factor // @Description: ECU FCR gradient/factor. Must be used along with _EFCR_OFT - // @Values: 0 - 1000 (0.1 Resolution) + // @Range: 0 1000 + // @Increment: 0.1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_EFCR_SLP", 52, AP_EFI, ecu_fcr_slope, 1), @@ -123,7 +125,8 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _EFCR_OFT // @DisplayName: ECU Fuel Consumption Rate Offset // @Description: ECU FCR intercept/offset. Must be used along with _EFCR_SLP - // @Values: 0 - 1000 (0.1 Resolution) + // @Range: 0 1000 + // @Increment: 0.1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_EFCR_OFT", 53, AP_EFI, ecu_fcr_offset, 0), @@ -131,7 +134,8 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _EFCR_AVG // @DisplayName: ECU Fuel Consumption Rate Average count // @Description: Averages _EFCR_AVG consecutive reading - // @Values: 0 - 100 (1 Resolution) + // @Range: 0 100 + // @Increment: 1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_EFCR_AVG", 54, AP_EFI, ecu_fcr_average_count, 1), @@ -139,7 +143,8 @@ const AP_Param::GroupInfo AP_EFI::var_info[] = { // @Param: _FUEL_VOL // @DisplayName: Full Fuel Volume / Capacity // @Description: Full fuel volume in ml - // @Values: 0 - 65535 (1 Resolution) + // @Range: 0 65535 + // @Increment: 1 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("_FUEL_VOL", 55, AP_EFI, fuel_volume_in_ml, 1), From ff1c1538b33da43e7ba1e0ec37bafde4d1bf51b9 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 9 Dec 2022 12:47:48 +0100 Subject: [PATCH 46/48] CI: update github actions to prevent nodejs version warning --- .github/workflows/cygwin_build.yml | 2 +- .github/workflows/macos_build.yml | 4 +- .github/workflows/test_ccache.yml | 2 +- .github/workflows/test_chibios.yml | 4 +- .github/workflows/test_coverage.yml | 4 +- .github/workflows/test_environment.yml | 2 +- .github/workflows/test_linux_sbc.yml | 4 +- .github/workflows/test_replay.yml | 4 +- .github/workflows/test_scripting_docs.yml | 45 +++++++++++++++++++++++ .github/workflows/test_sitl_copter.yml | 16 ++++---- .github/workflows/test_sitl_periph.yml | 8 ++-- .github/workflows/test_sitl_plane.yml | 8 ++-- .github/workflows/test_sitl_rover.yml | 8 ++-- .github/workflows/test_sitl_sub.yml | 8 ++-- .github/workflows/test_sitl_tracker.yml | 8 ++-- .github/workflows/test_size.yml | 6 +-- .github/workflows/test_unit_tests.yml | 4 +- 17 files changed, 91 insertions(+), 46 deletions(-) create mode 100644 .github/workflows/test_scripting_docs.yml diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 0d601df2fa..cd1f85216f 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -10,7 +10,7 @@ jobs: runs-on: 'windows-latest' steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' - uses: cygwin/cygwin-install-action@master diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index e142a796ac..a4a8b0a354 100644 --- a/.github/workflows/macos_build.yml +++ b/.github/workflows/macos_build.yml @@ -17,7 +17,7 @@ jobs: ] steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' - name: Install Prerequisites @@ -35,7 +35,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_ccache.yml b/.github/workflows/test_ccache.yml index dd5d970d5b..3f3ba047df 100644 --- a/.github/workflows/test_ccache.yml +++ b/.github/workflows/test_ccache.yml @@ -21,7 +21,7 @@ jobs: gcc: [10] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' - name: ccache test diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index faf560472c..a1c80b3c9d 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -59,7 +59,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -69,7 +69,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{ matrix.gcc }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index b700fa830e..5006033971 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -29,7 +29,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -39,7 +39,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_environment.yml b/.github/workflows/test_environment.yml index ee34b50aaf..8d7ba5c87d 100644 --- a/.github/workflows/test_environment.yml +++ b/.github/workflows/test_environment.yml @@ -62,7 +62,7 @@ jobs: esac # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' - name: test install environment ${{matrix.os}}.${{matrix.name}} diff --git a/.github/workflows/test_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml index e36a28fc3d..478922b005 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -43,7 +43,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -53,7 +53,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml index 7ecae879f4..d353061304 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -24,7 +24,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -34,7 +34,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_scripting_docs.yml b/.github/workflows/test_scripting_docs.yml new file mode 100644 index 0000000000..a281bc459e --- /dev/null +++ b/.github/workflows/test_scripting_docs.yml @@ -0,0 +1,45 @@ +name: test scripting docs + +on: + push: + paths: # only run for scripting changes + - 'libraries/AP_Scripting/tests/docs_check.py' + - 'libraries/AP_Scripting/generator/**' + + pull_request: + paths: # only run for scripting changes + - 'libraries/AP_Scripting/tests/docs_check.py' + - 'libraries/AP_Scripting/generator/**' + + workflow_dispatch: + +concurrency: + group: ci-${{github.workflow}}-${{ github.ref }} + cancel-in-progress: true + +jobs: + test-scripting-docs: + runs-on: ubuntu-20.04 + container: ardupilot/ardupilot-dev-base:latest + steps: + # git checkout the PR + - uses: actions/checkout@v3 + with: + submodules: 'recursive' + + - name: copy docs + run: | + PATH="/github/home/.local/bin:$PATH" + mv "libraries/AP_Scripting/docs/docs.lua" "libraries/AP_Scripting/docs/current_docs.lua" + + - name: build sitl # we don't really need to build the full code, just trigger docs re-gen with --scripting-docs, timeout after 10 seconds + shell: bash + run: | + PATH="/github/home/.local/bin:$PATH" + ./waf configure --board sitl + timeout 10 ./waf antennatracker --scripting-docs || code=$?; if [[ $code -ne 124 && $code -ne 0 ]]; then exit $code; fi + + - name: run compare + run: | + PATH="/github/home/.local/bin:$PATH" + python ./libraries/AP_Scripting/tests/docs_check.py "./libraries/AP_Scripting/docs/docs.lua" "./libraries/AP_Scripting/docs/current_docs.lua" diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index 28c72a64f0..46f48db74c 100644 --- a/.github/workflows/test_sitl_copter.yml +++ b/.github/workflows/test_sitl_copter.yml @@ -21,7 +21,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -31,7 +31,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -73,7 +73,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -83,7 +83,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -127,7 +127,7 @@ jobs: options: --privileged --cap-add=SYS_PTRACE --security-opt apparmor=unconfined --security-opt seccomp=unconfined steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -137,7 +137,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -167,7 +167,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -177,7 +177,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 8a517adf87..6144dda25d 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -14,7 +14,7 @@ jobs: container: ardupilot/ardupilot-dev-base:latest steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -24,7 +24,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -67,7 +67,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -77,7 +77,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 3640bc4e90..90f2262ae4 100644 --- a/.github/workflows/test_sitl_plane.yml +++ b/.github/workflows/test_sitl_plane.yml @@ -21,7 +21,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -31,7 +31,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -68,7 +68,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -78,7 +78,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index f14ffcf919..4773378396 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -21,7 +21,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -31,7 +31,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -69,7 +69,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -79,7 +79,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml index 39c25c28b5..f4357a6a23 100644 --- a/.github/workflows/test_sitl_sub.yml +++ b/.github/workflows/test_sitl_sub.yml @@ -21,7 +21,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -31,7 +31,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -67,7 +67,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -77,7 +77,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml index 8c6abec961..165ccb281d 100644 --- a/.github/workflows/test_sitl_tracker.yml +++ b/.github/workflows/test_sitl_tracker.yml @@ -21,7 +21,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -31,7 +31,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -67,7 +67,7 @@ jobs: steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -77,7 +77,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index ed38175a1f..534f3b2c90 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -26,7 +26,7 @@ jobs: f103-GPS # see special code for Periph below (3 places!) ] steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: ref: ${{ github.event.pull_request.base.ref }} path: base_branch @@ -38,7 +38,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -77,7 +77,7 @@ jobs: echo [`date`] Built ${{ github.event.pull_request.base.ref }} with no versions - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: fetch-depth: 0 path: 'pr' diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml index eba8083a9d..4b9afd5d83 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -29,7 +29,7 @@ jobs: ] steps: # git checkout the PR - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: submodules: 'recursive' # Put ccache into github cache for faster build @@ -39,7 +39,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "::set-output name=timestamp::${NOW}" - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} From b0cf07327c39764c95215516c806dbe905d92174 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 9 Dec 2022 12:48:00 +0100 Subject: [PATCH 47/48] CI: add ccache config to test_size --- .github/workflows/test_size.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index 534f3b2c90..47dae8b256 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -43,6 +43,9 @@ jobs: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on base branch + - name: setup ccache + run: | + . base_branch/.github/workflows/ccache.env - name: Build ${{ github.event.pull_request.base.ref }} ${{matrix.config}} ${{ matrix.toolchain }} env: CI_BUILD_TARGET: ${{matrix.config}} From 3596355735e0286994eaffdafe80e5d481fac55b Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 16 Jan 2023 16:28:24 +0100 Subject: [PATCH 48/48] CI: update github action to remove deprecation warnings from logs --- .github/workflows/cygwin_build.yml | 6 +++--- .github/workflows/macos_build.yml | 2 +- .github/workflows/test_chibios.yml | 2 +- .github/workflows/test_coverage.yml | 2 +- .github/workflows/test_linux_sbc.yml | 2 +- .github/workflows/test_replay.yml | 4 ++-- .github/workflows/test_sitl_copter.yml | 16 ++++++++-------- .github/workflows/test_sitl_periph.yml | 4 ++-- .github/workflows/test_sitl_plane.yml | 8 ++++---- .github/workflows/test_sitl_rover.yml | 8 ++++---- .github/workflows/test_sitl_sub.yml | 8 ++++---- .github/workflows/test_sitl_tracker.yml | 8 ++++---- .github/workflows/test_size.yml | 4 ++-- .github/workflows/test_unit_tests.yml | 4 ++-- 14 files changed, 39 insertions(+), 39 deletions(-) diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index cd1f85216f..3f5d35eb78 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -31,13 +31,13 @@ jobs: - name: Check build files id: check_files - uses: andstor/file-existence-action@v1 + uses: andstor/file-existence-action@v2 with: files: "artifacts/*" - allow_failure: true + fail: true - name: Archive build - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: binaries path: artifacts diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index a4a8b0a354..6b648caa96 100644 --- a/.github/workflows/macos_build.yml +++ b/.github/workflows/macos_build.yml @@ -33,7 +33,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index a1c80b3c9d..d6085ac5cb 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -67,7 +67,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index 5006033971..ed9bc392ef 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -37,7 +37,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: diff --git a/.github/workflows/test_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml index 478922b005..bb13d48dc7 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -51,7 +51,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml index d353061304..5923527b1b 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -32,7 +32,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -55,7 +55,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{ matrix.toolchain }}-${{matrix.config}} diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index 46f48db74c..271cb375d0 100644 --- a/.github/workflows/test_sitl_copter.yml +++ b/.github/workflows/test_sitl_copter.yml @@ -29,7 +29,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -81,7 +81,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -100,7 +100,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{matrix.config}} @@ -114,7 +114,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs @@ -135,7 +135,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -175,7 +175,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -194,7 +194,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{matrix.config}} @@ -208,7 +208,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 6144dda25d..b236cfcc28 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -22,7 +22,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -75,7 +75,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 90f2262ae4..e1903669b8 100644 --- a/.github/workflows/test_sitl_plane.yml +++ b/.github/workflows/test_sitl_plane.yml @@ -29,7 +29,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -76,7 +76,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -95,7 +95,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{matrix.config}} @@ -109,7 +109,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index 4773378396..98751a4c6a 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -29,7 +29,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -77,7 +77,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -96,7 +96,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{matrix.config}} @@ -110,7 +110,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml index f4357a6a23..c5685ce3e0 100644 --- a/.github/workflows/test_sitl_sub.yml +++ b/.github/workflows/test_sitl_sub.yml @@ -29,7 +29,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -75,7 +75,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -94,7 +94,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{matrix.config}} @@ -108,7 +108,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml index 165ccb281d..3ded077b4d 100644 --- a/.github/workflows/test_sitl_tracker.yml +++ b/.github/workflows/test_sitl_tracker.yml @@ -29,7 +29,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -75,7 +75,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -94,7 +94,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{matrix.config}} @@ -108,7 +108,7 @@ jobs: retention-days: 14 - name: Archive .bin artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: BIN-${{matrix.config}} path: /__w/ardupilot/ardupilot/logs diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index 47dae8b256..e576606291 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -36,7 +36,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -166,7 +166,7 @@ jobs: zip -r elf_diff.zip elf_diff - name: Archive elf_diff output - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 with: name: ELF_DIFF_${{matrix.config}} path: elf_diff.zip diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml index 4b9afd5d83..0dad924a80 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -37,7 +37,7 @@ jobs: id: ccache_cache_timestamp run: | NOW=$(date -u +"%F-%T") - echo "::set-output name=timestamp::${NOW}" + echo "{timestamp}=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files uses: actions/cache@v3 with: @@ -60,7 +60,7 @@ jobs: Tools/scripts/build_ci.sh - name: Archive buildlog artifacts - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() with: name: fail-${{ matrix.toolchain }}-${{matrix.config}}