diff --git a/README.md b/README.md index 0665aef3fa..c2909b1a9e 100644 --- a/README.md +++ b/README.md @@ -114,7 +114,7 @@ These boards are maintained to be compatible with PX4-Autopilot by the Manufactu ### Community supported -These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members. +These boards don't fully comply industry standards, and thus is solely maintained by the PX4 public community members. ### Experimental diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index af7ec2fbff..a6bdf9b780 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -165,7 +165,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then source $HOME/.profile # load changed path for the case the script is reran before relogin if [ $(which arm-none-eabi-gcc) ]; then GCC_VER_STR=$(arm-none-eabi-gcc --version) - GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}") + GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}" || true) fi if [[ "$GCC_FOUND_VER" == "1" ]]; then diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 5155fcf96c..d3ae62d8a4 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -81,32 +81,32 @@ else fi fi fi -fi -# Internal SPI bus ICM42688p -if ver hwtypecmp V6X009010 V6X010010 -then - icm42688p -R 12 -s start -else - if ver hwtypecmp V6X000010 - then - icm42688p -R 14 -s start - else - icm42688p -R 6 -s start - fi -fi - -if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 -then - # Internal SPI bus ICM-42670-P (hard-mounted) - icm42670p -R 10 -s start -else + # Internal SPI bus ICM42688p if ver hwtypecmp V6X009010 V6X010010 then - icm20602 -R 6 -s start + icm42688p -R 12 -s start else - # Internal SPI bus ICM-20649 (hard-mounted) - icm20649 -R 14 -s start + if ver hwtypecmp V6X000010 + then + icm42688p -R 14 -s start + else + icm42688p -R 6 -s start + fi + fi + + if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 + then + # Internal SPI bus ICM-42670-P (hard-mounted) + icm42670p -R 10 -s start + else + if ver hwtypecmp V6X009010 V6X010010 + then + icm20602 -R 6 -s start + else + # Internal SPI bus ICM-20649 (hard-mounted) + icm20649 -R 14 -s start + fi fi fi diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index 0de2a4311a..ccb3bb3ce9 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -16,6 +16,8 @@ CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL=y +CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ=25000000 CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 03845239ef..a3d9b02338 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -136,7 +136,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char #if defined(PX4_LOG_COLORIZED_OUTPUT) if (use_color) { - // alway reset color + // always reset color const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1); pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s\n", PX4_ANSI_COLOR_RESET); diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 9c17cab378..473c554866 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -76,7 +76,7 @@ void Subscription::unsubscribe() bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { - if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) { + if (uORB::Manager::orb_device_node_exists(_orb_id, instance)) { // if desired new instance exists, unsubscribe from current unsubscribe(); _instance = instance; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 5a72cc9f5e..4246ea3585 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -898,11 +898,13 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann case IOTimerChanMode_Dshot: dier_bit = 0; - cr1_bit = state ? GTIM_CR1_CEN : 0; - break; - case IOTimerChanMode_PWMIn: + /* fallthrough */ case IOTimerChanMode_Capture: + cr1_bit = state ? GTIM_CR1_CEN : 0; + + /* fallthrough */ + case IOTimerChanMode_PWMIn: break; default: diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index 087ffce6c8..e6e7f236f7 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -350,16 +350,12 @@ void MspOsd::Run() home_position_s home_position{}; _home_position_sub.copy(&home_position); - estimator_status_s estimator_status{}; - _estimator_status_sub.copy(&estimator_status); - vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); // construct and send message const auto msg = msp_osd::construct_COMP_GPS( home_position, - estimator_status, vehicle_global_position, _heartbeat); this->Send(MSP_COMP_GPS, &msg); @@ -379,17 +375,11 @@ void MspOsd::Run() sensor_gps_s vehicle_gps_position{}; _vehicle_gps_position_sub.copy(&vehicle_gps_position); - estimator_status_s estimator_status{}; - _estimator_status_sub.copy(&estimator_status); - vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); // construct and send message - const auto msg = msp_osd::construct_ALTITUDE( - vehicle_gps_position, - estimator_status, - vehicle_local_position); + const auto msg = msp_osd::construct_ALTITUDE(vehicle_gps_position, vehicle_local_position); this->Send(MSP_ALTITUDE, &msg); } diff --git a/src/drivers/osd/msp_osd/msp_osd.hpp b/src/drivers/osd/msp_osd/msp_osd.hpp index ad233e2041..87918c7864 100644 --- a/src/drivers/osd/msp_osd/msp_osd.hpp +++ b/src/drivers/osd/msp_osd/msp_osd.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -145,7 +144,6 @@ private: // subscriptions to desired vehicle display information uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; - uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _input_rc_sub{ORB_ID(input_rc)}; uORB::Subscription _log_message_sub{ORB_ID(log_message)}; diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 1a78179855..e2cf9daf5e 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -365,7 +365,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, } msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, - const estimator_status_s &estimator_status, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat) { @@ -375,7 +374,8 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, // Calculate distance and direction to home if (home_position.valid_hpos && home_position.valid_lpos - && estimator_status.solution_status_flags & (1 << 4)) { + && (hrt_elapsed_time(&vehicle_global_position.timestamp) < 1_s)) { + float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, home_position.lat, home_position.lon)); @@ -425,7 +425,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude) } msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, - const estimator_status_s &estimator_status, const vehicle_local_position_s &vehicle_local_position) { // initialize result @@ -438,7 +437,7 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, altitude.estimatedActualPosition = 0; } - if (estimator_status.solution_status_flags & (1 << 5)) { + if (vehicle_local_position.v_z_valid) { altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s } else { diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.hpp b/src/drivers/osd/msp_osd/uorb_to_msp.hpp index d841faf522..1f5cdb3b6d 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.hpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.hpp @@ -54,7 +54,6 @@ #include #include #include -#include #include #include @@ -94,7 +93,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, // construct an MSP_COMP_GPS struct msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, - const estimator_status_s &estimator_status, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat); @@ -103,7 +101,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude); // construct an MSP_ALTITUDE struct msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, - const estimator_status_s &estimator_status, const vehicle_local_position_s &vehicle_local_position); // construct an MSP_ESC_SENSOR_DATA struct diff --git a/src/drivers/pca9685_pwm_out/Kconfig b/src/drivers/pca9685_pwm_out/Kconfig index 84cc71cda7..ad7c25b8d7 100644 --- a/src/drivers/pca9685_pwm_out/Kconfig +++ b/src/drivers/pca9685_pwm_out/Kconfig @@ -2,4 +2,20 @@ menuconfig DRIVERS_PCA9685_PWM_OUT bool "pca9685_pwm_out" default n ---help--- - Enable support for pca9685_pwm_out \ No newline at end of file + Enable support for pca9685_pwm_out + +if DRIVERS_PCA9685_PWM_OUT + config PCA9685_USE_EXTERNAL_CRYSTAL + bool "Use external crystal for clock reference" + default n + + config PCA9685_EXTERNAL_CRYSTAL_FREQ + int "External crystal frequency" + depends on PCA9685_USE_EXTERNAL_CRYSTAL + default 25000000 + + config PCA9685_INTERNAL_CRYSTAL_FREQ + int "Corrected frequency of internal oscillator" + depends on !PCA9685_USE_EXTERNAL_CRYSTAL + default 26075000 +endif \ No newline at end of file diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index c2a1af4b16..7eccd6c04b 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -32,23 +32,66 @@ ****************************************************************************/ #include +#include #include #include "PCA9685.h" -#include + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ +#else +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_INTERNAL_CRYSTAL_FREQ +#endif + +#define PCA9685_DEFAULT_MODE1_CFG PCA9685_MODE1_AI_MASK +#define PCA9685_DEFAULT_MODE2_CFG PCA9685_MODE2_OUTDRV_MASK using namespace drv_pca9685_pwm; PCA9685::PCA9685(int bus, int addr): - I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000) + I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000), + currentFreq(0.0) { } -int PCA9685::Stop() +int PCA9685::init() { - disableAllOutput(); - stopOscillator(); + int ret = I2C::init(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {}; + + buf[0] = PCA9685_REG_MODE1; + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK; // put into sleep mode + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK | PCA9685_MODE1_EXTCLK_MASK; + ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#endif + + buf[0] = PCA9685_REG_MODE2; + buf[1] = PCA9685_DEFAULT_MODE2_CFG; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + return PX4_OK; } @@ -63,198 +106,118 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) memcpy(out, outputs, sizeof(uint16_t) * num_outputs); for (unsigned i = 0; i < num_outputs; ++i) { - out[i] = (uint16_t)roundl((out[i] * _Freq * PCA9685_PWM_RES / (float)1e6)); // convert us to 12 bit resolution + out[i] = calcRawFromPulse(out[i]); } - setPWM(num_outputs, out); - - return 0; + return writePWM(0, out, num_outputs); } -int PCA9685::setFreq(float freq) +int PCA9685::updateFreq(float freq) { - uint16_t realResolution = floorl((float)PCA9685_CLOCK_FREQ / freq); + uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_REFERENCE / freq / PCA9685_PWM_RES) - 1; - if (realResolution < PCA9685_PWM_RES) { // unable to provide enough resolution - PX4_DEBUG("frequency too high"); - return -EINVAL; + if (divider > 0x00FF) { + PX4_ERR("frequency too low"); + return PX4_ERROR; } - uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; - - if (divider > 0x00FF) { // out of divider - PX4_DEBUG("frequency too low"); - return -EINVAL; + if (divider < 0x0003) { + PX4_ERR("frequency too high"); + return PX4_ERROR; } - float freq_err = ((float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) - - (float)(freq * PCA9685_PWM_RES)) - / (float)(freq * PCA9685_PWM_RES); // actually asked for (freq * PCA9685_PWM_RES) - - if (fabsf(freq_err) > 0.01f) { // TODO decide threshold - PX4_WARN("frequency error too large: %.4f", (double)freq_err); - // should we return an error? - } - - _Freq = (float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) / PCA9685_PWM_RES; // use actual pwm freq instead. - - setDivider(divider); - - return PX4_OK; + currentFreq = (float)PCA9685_CLOCK_REFERENCE / (float)((divider + 1) * 4096); + PX4_INFO("PCA9685 PWM frequency: target=%.2f real=%.2f", (double)freq, (double)currentFreq); + return setDivider(divider); } -int PCA9685::initReg() +int PCA9685::updateRAW(const uint16_t *outputs, unsigned int num_outputs) { - uint8_t buf[2] = {}; + return writePWM(0, outputs, num_outputs); +} - buf[0] = PCA9685_REG_MODE1; - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); // make sure oscillator is disabled +int PCA9685::setAllPWM(uint16_t output) +{ + uint16_t val = (uint16_t)roundl((output * currentFreq * PCA9685_PWM_RES / (float)1e6)); + uint8_t buf[] = { + PCA9685_REG_ALLLED_ON_L, + 0x00, 0x00, + (uint8_t)(val & (uint8_t)0xFF), + val != 0 ? (uint8_t)(val >> 8) : (uint8_t)PCA9685_LED_ON_FULL_ON_OFF_MASK + }; + return transfer(buf, sizeof(buf), nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } +int PCA9685::sleep() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK + }; + return transfer(buf, 2, nullptr, 0); +} - ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible +int PCA9685::wake() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG + }; + return transfer(buf, 2, nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } - - buf[0] = PCA9685_REG_MODE2; - buf[1] = DEFAULT_MODE2_CFG; - ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } - - return PX4_OK; +int PCA9685::doRestart() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_RESTART_MASK + }; + return transfer(buf, 2, nullptr, 0); } int PCA9685::probe() { - return I2C::probe(); + int ret = I2C::probe(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {0x00}; + return transfer(buf, 2, buf, 1); } -void PCA9685::setPWM(uint8_t channel, const uint16_t &value) -{ - if (value >= 4096) { - PX4_DEBUG("invalid pwm value"); - return; - } - - uint8_t buf[5] = {}; - buf[0] = PCA9685_REG_LED0 + channel * PCA9685_REG_LED_INCREMENT; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = (uint8_t)(value & (uint8_t)0xFF); - buf[4] = value != 0 ? ((uint8_t)(value >> (uint8_t)8)) : PCA9685_LED_ON_FULL_ON_OFF_MASK; - - int ret = transfer(buf, 5, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("setPWM: i2c::transfer returned %d", ret); - } -} - -void PCA9685::setPWM(uint8_t channel_count, const uint16_t *value) +int PCA9685::writePWM(uint8_t idx, const uint16_t *value, uint8_t num) { uint8_t buf[PCA9685_PWM_CHANNEL_COUNT * PCA9685_REG_LED_INCREMENT + 1] = {}; - buf[0] = PCA9685_REG_LED0; - - for (int i = 0; i < channel_count; ++i) { - if (value[i] >= 4096) { - PX4_DEBUG("invalid pwm value"); - return; - } + buf[0] = PCA9685_REG_LED0 + PCA9685_REG_LED_INCREMENT * idx; + for (int i = 0; i < num; ++i) { buf[1 + i * PCA9685_REG_LED_INCREMENT] = 0x00; - buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; - buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & (uint8_t)0xFF); - buf[4 + i * PCA9685_REG_LED_INCREMENT] = value[i] != 0 ? ((uint8_t)(value[i] >> (uint8_t)8)) : - PCA9685_LED_ON_FULL_ON_OFF_MASK; + + if (value[i] == 0) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + + } else if (value[i] == 4096) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + + } else { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & 0xFF); + buf[4 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] >> 8); + } } - int ret = transfer(buf, channel_count * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("setPWM: i2c::transfer returned %d", ret); - } + return transfer(buf, num * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); } -void PCA9685::disableAllOutput() -{ - uint8_t buf[5] = {}; - buf[0] = PCA9685_REG_ALLLED_ON_L; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = PCA9685_LED_ON_FULL_ON_OFF_MASK; - - int ret = transfer(buf, 5, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - } -} - -void PCA9685::setDivider(uint8_t value) +int PCA9685::setDivider(uint8_t value) { uint8_t buf[2] = {}; buf[0] = PCA9685_REG_PRE_SCALE; buf[1] = value; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::stopOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // set to sleep - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::startOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("startOscillator: i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::triggerRestart() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - buf[1] |= PCA9685_MODE1_RESTART_MASK; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret); - return; - } + return transfer(buf, 2, nullptr, 0); } diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h index 5c7740164c..e8bd5eb4d8 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -34,10 +34,7 @@ #pragma once #include #include -#include - -namespace drv_pca9685_pwm -{ +#include #define PCA9685_REG_MODE1 0x00 // Mode register 1 #define PCA9685_REG_MODE2 0x01 // Mode register 2 @@ -82,93 +79,98 @@ namespace drv_pca9685_pwm // PRE_SCALE register #define PCA9685_PRE_SCALE_MASK 0xFF +// common sense #define PCA9685_PWM_CHANNEL_COUNT 16 -#define PCA9685_PWM_RES 4096 //Resolution 4096=12bit -/* This should be 25000000 ideally, - * but it seems most chips have its oscillator working at a higher frequency - * Reference: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/blob/6664ce936210eea53259b814062009d9569a4213/Adafruit_PWMServoDriver.h#L66 */ -#define PCA9685_CLOCK_INT 26075000.0 //25MHz internal clock -#ifndef PCA9685_CLOCK_EXT -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk -#else -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk -#endif +#define PCA9685_PWM_RES 4096 -#define PCA9685_DEVICE_BASE_PATH "/dev/pca9685" -#define PWM_DEFAULT_FREQUENCY 50 // default pwm frequency +namespace drv_pca9685_pwm +{ //! Main class that exports features for PCA9685 chip class PCA9685 : public device::I2C { public: PCA9685(int bus, int addr); + ~PCA9685() override = default; - int Stop(); + int init() override; /* - * outputs formatted to us. + * Write new PWM value to device + * + * *output: pulse width, us */ int updatePWM(const uint16_t *outputs, unsigned num_outputs); - int setFreq(float freq); - - ~PCA9685() override = default; - - int initReg(); - - inline float getFrequency() {return _Freq;} - /* - * disable all of the output + * Set PWM frequency to new value. + * + * Only a few of precious frequency can be set, while others will be rounded to the nearest possible value. + * + * Only allowed when PCA9685 is put into sleep mode + * + * freq: Hz */ - void disableAllOutput(); + int updateFreq(float freq); /* - * turn off oscillator - */ - void stopOscillator(); - - /* - * turn on oscillator + * Write new PWM value to device, in raw counter value + * + * *output: 0~4095 */ - void startOscillator(); + int updateRAW(const uint16_t *outputs, unsigned num_outputs); /* - * turn on output + * Get the real frequency */ - void triggerRestart(); + float inline getFreq() {return currentFreq;} + + uint16_t inline calcRawFromPulse(uint16_t pulse_width) + { + return (uint16_t)roundl((pulse_width * currentFreq * PCA9685_PWM_RES / (float)1e6)); + } + + /* + * Set PWM value on all channels at once + */ + int setAllPWM(uint16_t output); + + /* + * Put PCA9685 into sleep mode + * + * This will disable the clock reference inside PCA9685 + */ + int sleep(); + + /* + * Put PCA9685 out of sleep mode. + * + * Must wait 500 us for oscillator stabilization before outputting anything + */ + int wake(); + + /* + * If PCA9685 is put into sleep without clearing all the outputs, + * then the restart command will be available, and it can bring back PWM output without the + * need of updatePWM() call. + */ + int doRestart(); protected: int probe() override; -#ifdef PCA9685_CLOCL_EXT - static const uint8_t DEFAULT_MODE1_CFG = 0x70; // Auto-Increment, Sleep, EXTCLK -#else - static const uint8_t DEFAULT_MODE1_CFG = 0x30; // Auto-Increment, Sleep -#endif - static const uint8_t DEFAULT_MODE2_CFG = 0x04; // totem pole - - float _Freq = PWM_DEFAULT_FREQUENCY; - - /** - * set PWM value for a channel[0,15]. - * value should be range of 0-4095 - */ - void setPWM(uint8_t channel, const uint16_t &value); - - /** - * set all PWMs in a single I2C transmission. - * value should be range of 0-4095 - */ - void setPWM(uint8_t channel_count, const uint16_t *value); - /* * set clock divider */ - void setDivider(uint8_t value); + int setDivider(uint8_t value); + + /* + * Write PWM value to PCA9685 + */ + int writePWM(uint8_t idx, const uint16_t *value, uint8_t num); private: - + float currentFreq; }; } diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 4c3ee60c54..0e9bfa63df 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -33,9 +33,10 @@ /** * @file pca9685/main.cpp - * A cross-platform driver and wrapper for pca9685. - * Designed to support all control-groups by binding to correct mixer files - * @author SalimTerryLi + * + * This file serves as the wrapper layer for PCA9685 driver, working with parameters + * and scheduling stuffs on PX4 side. + * */ #include @@ -45,11 +46,10 @@ #include #include #include +#include #include "PCA9685.h" -#include - #define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_ADDRESS (0x40) @@ -59,26 +59,25 @@ using namespace time_literals; class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface { public: - PCA9685Wrapper(int schd_rate_limit = 400); - ~PCA9685Wrapper() override ; + PCA9685Wrapper(); + ~PCA9685Wrapper() override; + PCA9685Wrapper(const PCA9685Wrapper &) = delete; + PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; int init(); - /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); - /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override; - PCA9685Wrapper(const PCA9685Wrapper &) = delete; - PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; - int print_status() override; +protected: + void updateParams() override; + private: perf_counter_t _cycle_perf; @@ -86,40 +85,37 @@ private: INIT, WAIT_FOR_OSC, RUNNING - }; - STATE _state{STATE::INIT}; - // used to compare and cancel unecessary scheduling changes caused by parameter update - int32_t _last_fetched_Freq = -1; - // If this value is above zero, then change freq and scheduling in running state. - float _targetFreq = -1.0f; + } state{STATE::INIT}; + PCA9685 *pca9685 = nullptr; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + MixingOutput _mixing_output { + "PCA9685", + PCA9685_PWM_CHANNEL_COUNT, + *this, + MixingOutput::SchedulingPolicy::Disabled, + true + }; + + float param_pwm_freq, previous_pwm_freq; + float param_schd_rate, previous_schd_rate; + uint32_t param_duty_mode; void Run() override; - -protected: - int _schd_rate_limit = 400; - - PCA9685 *pca9685 = nullptr; // driver handle. - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - MixingOutput _mixing_output{"PCA9685", PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true}; }; -PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) : +PCA9685Wrapper::PCA9685Wrapper() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), - _schd_rate_limit(schd_rate_limit) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { } PCA9685Wrapper::~PCA9685Wrapper() { - if (pca9685 != nullptr) { // normally this should not be called. - PX4_DEBUG("Destruction of PCA9685Wrapper without pwmDevice unloaded!"); - pca9685->Stop(); // force stop + if (pca9685 != nullptr) { + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; - pca9685 = nullptr; } perf_free(_cycle_perf); @@ -129,18 +125,7 @@ int PCA9685Wrapper::init() { int ret = pca9685->init(); - if (ret != PX4_OK) { - return ret; - } - - param_t param_h = param_find("PCA9685_RATE"); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &_targetFreq); - - } else { - PX4_DEBUG("PARAM_INVALID: PCA9685_RATE"); - } + if (ret != PX4_OK) { return ret; } this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id())); @@ -154,7 +139,26 @@ int PCA9685Wrapper::init() bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { - return pca9685->updatePWM(outputs, num_outputs) == 0 ? true : false; + if (state != STATE::RUNNING) { return false; } + + uint16_t low_level_outputs[PCA9685_PWM_CHANNEL_COUNT] = {}; + num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs; + + for (uint8_t i = 0; i < num_outputs; ++i) { + if (param_duty_mode & (1 << i)) { + low_level_outputs[i] = outputs[i]; + + } else { + low_level_outputs[i] = pca9685->calcRawFromPulse(outputs[i]); + } + } + + if (pca9685->updateRAW(low_level_outputs, num_outputs) != PX4_OK) { + PX4_ERR("Failed to write PWM to PCA9685"); + return false; + } + + return true; } void PCA9685Wrapper::Run() @@ -163,7 +167,8 @@ void PCA9685Wrapper::Run() ScheduleClear(); _mixing_output.unregister(); - pca9685->Stop(); + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; pca9685 = nullptr; @@ -171,44 +176,27 @@ void PCA9685Wrapper::Run() return; } - perf_begin(_cycle_perf); - - switch (_state) { + switch (state) { case STATE::INIT: - pca9685->initReg(); + updateParams(); + pca9685->updateFreq(param_pwm_freq); + previous_pwm_freq = param_pwm_freq; + previous_schd_rate = param_schd_rate; - if (_targetFreq > 0.0f) { - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency to %.2f, fall back to 50Hz", (double)_targetFreq); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - - } else { - // should not happen - PX4_ERR("INIT failed: invalid initial frequency settings"); - } - - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; + pca9685->wake(); + state = STATE::WAIT_FOR_OSC; ScheduleDelayed(500); break; case STATE::WAIT_FOR_OSC: { - pca9685->triggerRestart(); // start actual outputting - _state = STATE::RUNNING; - float schedule_rate = pca9685->getFrequency(); - - if (_schd_rate_limit < pca9685->getFrequency()) { - schedule_rate = _schd_rate_limit; - } - - ScheduleOnInterval(1000000 / schedule_rate, 1000000 / schedule_rate); + state = STATE::RUNNING; + ScheduleOnInterval(1000000 / param_schd_rate, 0); } break; case STATE::RUNNING: + perf_begin(_cycle_perf); + _mixing_output.update(); // check for parameter updates @@ -219,30 +207,36 @@ void PCA9685Wrapper::Run() // update parameters from storage updateParams(); + + // apply param updates + if ((float)fabs(previous_pwm_freq - param_pwm_freq) > 0.01f) { + previous_pwm_freq = param_pwm_freq; + + ScheduleClear(); + + pca9685->sleep(); + pca9685->updateFreq(param_pwm_freq); + pca9685->wake(); + + // update of PWM freq will always trigger scheduling change + previous_schd_rate = param_schd_rate; + + state = STATE::WAIT_FOR_OSC; + ScheduleDelayed(500); + + } else if ((float)fabs(previous_schd_rate - param_schd_rate) > 0.01f) { + // case when PWM freq not changed but scheduling rate does + previous_schd_rate = param_schd_rate; + ScheduleClear(); + ScheduleOnInterval(1000000 / param_schd_rate, 1000000 / param_schd_rate); + } } _mixing_output.updateSubscriptions(false); - if (_targetFreq > 0.0f) { // check if frequency should be changed - ScheduleClear(); - pca9685->disableAllOutput(); - pca9685->stopOscillator(); - - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency, fall back to 50Hz"); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; - ScheduleDelayed(500); - } - + perf_end(_cycle_perf); break; } - - perf_end(_cycle_perf); } int PCA9685Wrapper::print_usage(const char *reason) @@ -254,27 +248,24 @@ int PCA9685Wrapper::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module is responsible for generate pwm pulse with PCA9685 chip. +This is a PCA9685 PWM output driver. -It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. +It runs on I2C workqueue which is asynchronous with FC control loop, +fetching the latest mixing result and write them to PCA9685 at its scheduling ticks. -### Implementation -This module depends on ModuleBase and OutputModuleInterface. -IIC communication is based on CDev::I2C +It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width +that can be accepted by most ESCs and servos. ### Examples It is typically started with: -$ pca9685_pwm_out start -a 64 -b 1 +$ pca9685_pwm_out start -a 0x40 -b 1 -The number X can be acquired by executing -`pca9685_pwm_out status` when this driver is running. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("pca9685_pwm_out", "driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); - PRINT_MODULE_USAGE_PARAM_INT('a',64,0,255,"device address on this bus",true); + PRINT_MODULE_USAGE_PARAM_STRING('a',"0x40","","7-bits I2C address of PCA9685",true); PRINT_MODULE_USAGE_PARAM_INT('b',1,0,255,"bus that pca9685 is connected to",true); - PRINT_MODULE_USAGE_PARAM_INT('r',400,50,400,"schedule rate limit",true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; @@ -282,39 +273,42 @@ The number X can be acquired by executing int PCA9685Wrapper::print_status() { int ret = ModuleBase::print_status(); - PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, true frequency %.5f", + PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, real frequency %.2f", pca9685->get_device_bus(), pca9685->get_device_address(), - (double)(pca9685->getFrequency())); + (double)(pca9685->getFreq())); return ret; } -int PCA9685Wrapper::custom_command(int argc, char **argv) { // only for test use +int PCA9685Wrapper::custom_command(int argc, char **argv) { return PX4_OK; } int PCA9685Wrapper::task_spawn(int argc, char **argv) { - int ch; int address=PCA9685_DEFAULT_ADDRESS; int iicbus=PCA9685_DEFAULT_IICBUS; - int schd_rate_limit=400; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "a:b:r:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': - address = atoi(myoptarg); + errno = 0; + address = strtol(myoptarg, nullptr, 16); + if (errno != 0) { + PX4_WARN("Invalid address"); + return PX4_ERROR; + } break; case 'b': - iicbus = atoi(myoptarg); - break; - - case 'r': - schd_rate_limit = atoi(myoptarg); + iicbus = strtol(myoptarg, nullptr, 10); + if (errno != 0) { + PX4_WARN("Invalid bus"); + return PX4_ERROR; + } break; case '?': @@ -326,7 +320,7 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { } } - auto *instance = new PCA9685Wrapper(schd_rate_limit); + auto *instance = new PCA9685Wrapper(); if (instance) { _object.store(instance); @@ -358,6 +352,31 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { return PX4_ERROR; } +void PCA9685Wrapper::updateParams() { + ModuleParams::updateParams(); + + param_t param = param_find("PCA9685_SCHD_HZ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_schd_rate); + } else { + PX4_ERR("param PCA9685_SCHD_HZ not found"); + } + + param = param_find("PCA9685_PWM_FREQ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_pwm_freq); + } else { + PX4_ERR("param PCA9685_PWM_FREQ not found"); + } + + param = param_find("PCA9685_DUTY_EN"); + if (param != PARAM_INVALID) { + param_get(param, (int32_t*)¶m_duty_mode); + } else { + PX4_ERR("param PCA9685_DUTY_EN not found"); + } +} + extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){ return PCA9685Wrapper::main(argc, argv); } diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml index 78801fbdb6..198810f8cb 100644 --- a/src/drivers/pca9685_pwm_out/module.yaml +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -5,23 +5,75 @@ actuator_output: channel_label: 'Channel' standard_params: disarmed: { min: 800, max: 2200, default: 1000 } - min: { min: 800, max: 1400, default: 1000 } - max: { min: 1600, max: 2200, default: 2000 } + min: { min: 800, max: 1400, default: 1100 } + max: { min: 1600, max: 2200, default: 1900 } failsafe: { min: 800, max: 2200 } + custom_params: + - name: 'DUTY_EN' + label: "Duty-Cycle\n Mode" + index_offset: -1 + show_as: bitset + advanced: true num_channels: 16 + parameters: - - group: PCA9685 - definitions: - PCA9685_RATE: - description: - short: PWM frequency for PCA9685 - long: | - Update rate at which the PWM signal is sent to the ESC. - type: float - decimal: 1 - increment: 0.1 - default: 50 - min: 50 - max: 450 - unit: Hz - reboot_required: true + - group: Actuator Outputs + definitions: + PCA9685_SCHD_HZ: + description: + short: PWM update rate + long: | + Controls the update rate of PWM output. + Flight Controller will inform those numbers of update events in a second, to PCA9685. + Higher update rate will consume more I2C bandwidth, which may even lead to worse + output latency, or completely block I2C bus. + type: float + decimal: 2 + min: 50.0 + max: 400.0 + default: 50.0 + PCA9685_PWM_FREQ: + description: + short: PWM cycle frequency + long: | + Controls the PWM frequency at timing perspective. + This is independent from PWM update frequency, as PCA9685 is capable to output + without being continuously commanded by FC. + Higher frequency leads to more accurate pulse width, but some ESCs and servos may not support it. + This parameter should be set to the same value as PWM update rate in most case. + This parameter MUST NOT exceed upper limit of 400.0, if any outputs as generic 1000~2000us + pulse width is desired. Frequency higher than 400 only makes sense in duty-cycle mode. + type: float + decimal: 2 + min: 23.8 + max: 1525.87 + default: 50.0 + PCA9685_DUTY_EN: + description: + short: Put the selected channels into Duty-Cycle output mode + long: | + The driver will output standard pulse-width encoded signal without this bit set. + To make PCA9685 output in duty-cycle fashion, please enable the corresponding + channel bit here and adjusting standard params to suit your need. + The driver will have 12bits resolution for duty-cycle output. That means to achieve 0% to 100% + output range on one channel, the corresponding params MIN and MAX for the channel should be set + to 0 and 4096. Other standard params follows the same rule. + type: bitmask + bit: + 0: Put CH1 to Duty-Cycle mode + 1: Put CH2 to Duty-Cycle mode + 2: Put CH3 to Duty-Cycle mode + 3: Put CH4 to Duty-Cycle mode + 4: Put CH5 to Duty-Cycle mode + 5: Put CH6 to Duty-Cycle mode + 6: Put CH7 to Duty-Cycle mode + 7: Put CH8 to Duty-Cycle mode + 8: Put CH9 to Duty-Cycle mode + 9: Put CH10 to Duty-Cycle mode + 10: Put CH11 to Duty-Cycle mode + 11: Put CH12 to Duty-Cycle mode + 12: Put CH13 to Duty-Cycle mode + 13: Put CH14 to Duty-Cycle mode + 14: Put CH15 to Duty-Cycle mode + 15: Put CH16 to Duty-Cycle mode + default: 0 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 8210be8280..1094ff33c1 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) +add_subdirectory(atmosphere EXCLUDE_FROM_ALL) add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 24f7f01850..4d29be4a00 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -44,6 +44,9 @@ #include #include +#include + +using atmosphere::getDensityFromPressureAndTemp; float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius) @@ -53,7 +56,7 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ } // air density in kg/m3 - const float rho_air = get_air_density(pressure_ambient, temperature_celsius); + const float rho_air = getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius); const float dp = fabsf(differential_pressure); float dp_tot = dp; @@ -153,18 +156,6 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ break; } - // if (!PX4_ISFINITE(dp_tube)) { - // dp_tube = 0.0f; - // } - - // if (!PX4_ISFINITE(dp_pitot)) { - // dp_pitot = 0.0f; - // } - - // if (!PX4_ISFINITE(dv)) { - // dv = 0.0f; - // } - // computed airspeed without correction for inflow-speed at tip of pitot-tube const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); @@ -192,7 +183,7 @@ float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float te temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius } - return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius)); } @@ -203,7 +194,7 @@ float calc_CAS_from_IAS(float speed_indicated, float scale) float calc_TAS(float total_pressure, float static_pressure, float temperature_celsius) { - float density = get_air_density(static_pressure, temperature_celsius); + float density = getDensityFromPressureAndTemp(static_pressure, temperature_celsius); if (density < 0.0001f || !PX4_ISFINITE(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; @@ -219,15 +210,6 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce } } -float get_air_density(float static_pressure, float temperature_celsius) -{ - if (!PX4_ISFINITE(temperature_celsius)) { - temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius - } - - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} - float calc_calibrated_from_true_airspeed(float speed_true, float air_density) { return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index dba000a4e5..6b5161c5dd 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -127,7 +127,6 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe * @param static_pressure ambient pressure in millibar * @param temperature_celcius air / ambient temperature in Celsius */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); /** * @brief Calculates calibrated airspeed from true airspeed and air density diff --git a/src/lib/atmosphere/CMakeLists.txt b/src/lib/atmosphere/CMakeLists.txt new file mode 100644 index 0000000000..eb25c8df4b --- /dev/null +++ b/src/lib/atmosphere/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(atmosphere + atmosphere.cpp + ) +px4_add_unit_gtest(SRC test_atmosphere.cpp LINKLIBS atmosphere) diff --git a/src/lib/atmosphere/atmosphere.cpp b/src/lib/atmosphere/atmosphere.cpp new file mode 100644 index 0000000000..66b98626f7 --- /dev/null +++ b/src/lib/atmosphere/atmosphere.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file atmosphere.cpp + * + */ + +#include +#include "atmosphere.h" +namespace atmosphere +{ + +static constexpr float kTempRefKelvin = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin +static constexpr float kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter +static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in Pa + +float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius) +{ + return (pressure_pa / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS))); +} +float getPressureFromAltitude(const float altitude_m) +{ + + return kPressRefSeaLevelPa * powf((altitude_m * kTempGradient + kTempRefKelvin) / kTempRefKelvin, + -CONSTANTS_ONE_G / (kTempGradient * CONSTANTS_AIR_GAS_CONST)); +} +float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa) +{ + // calculate altitude using the hypsometric equation + + const float pressure_ratio = pressure_pa / pressure_sealevel_pa; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + return (((powf(pressure_ratio, (-(kTempGradient * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * kTempRefKelvin) - + kTempRefKelvin) / kTempGradient; + +} +float getStandardTemperatureAtAltitude(float altitude_m) +{ + return 15.0f + kTempGradient * altitude_m; +} +} \ No newline at end of file diff --git a/src/lib/atmosphere/atmosphere.h b/src/lib/atmosphere/atmosphere.h new file mode 100644 index 0000000000..849a430347 --- /dev/null +++ b/src/lib/atmosphere/atmosphere.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file atmosphere.h + * + */ + +#ifndef PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ +#define PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ + +namespace atmosphere +{ + +// NOTE: We are currently only modelling the first layer of the US Standard Atmosphere 1976. +// This means that the functions are only valid up to an altitude of 11km. + +/** +* Calculate air density given air pressure and temperature. +* +* @param pressure_pa ambient pressure in Pa +* @param temperature_celsius ambient temperature in degrees Celsius +*/ +float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius); + +/** +* Calculate standard airpressure given altitude. +* +* @param altitude_m altitude above MSL in meters in the standard atmosphere +*/ +float getPressureFromAltitude(const float altitude_m); + +/** +* Calculate altitude from air pressure and temperature. +* +* @param pressure_pa ambient pressure in Pa +* @param pressure_sealevel_pa sea level pressure in Pa +*/ +float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa); + +/** +* Get standard temperature at altitude. +* +* @param altitude_m Altitude msl in meters +* @return Standard temperature in degrees Celsius +*/ +float getStandardTemperatureAtAltitude(float altitude_m); +} + +#endif //PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ diff --git a/src/lib/atmosphere/test_atmosphere.cpp b/src/lib/atmosphere/test_atmosphere.cpp new file mode 100644 index 0000000000..0a79c89ca1 --- /dev/null +++ b/src/lib/atmosphere/test_atmosphere.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * To run this test only use: make tests TESTFILTER=atmosphere + */ + +#include +#include +using namespace atmosphere; + +TEST(TestAtmosphere, pressureFromAltitude) +{ + // GIVEN pressure at seal level in standard atmosphere and sea level altitude + const float pressureAtSeaLevel = 101325.f; // Pa + float altitude = 0.0f; + + // WHEN we calculate pressure based on altitude + float pressure = getPressureFromAltitude(altitude); + + // THEN expect seal level pressure for standard atmosphere + EXPECT_FLOAT_EQ(pressure, pressureAtSeaLevel); + + // WHEN we are at 3000m altitude + altitude = 3000.0f; + pressure = getPressureFromAltitude(altitude); + + // THEN expect standard atmosphere pressure at 3000m (error of 10Pa corresponds to 1m altitude error in standard atmosphere when altitude is between 1000 and 2000m) + EXPECT_NEAR(pressure, 70120.f, 10.0f); +} + +TEST(TestAtmosphere, altitudeFromPressure) +{ + // GIVEN pressure at seal level in standard atmosphere + const float pressureAtSealevel = 101325.f; + float pressure = pressureAtSealevel; + + // WHEN we calculate altitude from pressure + float altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect resulting altitude to be near sea level + EXPECT_FLOAT_EQ(altitude, 0.0f); + + // GIVEN pressure is standard atmosphere pressure at 3000m + pressure = 70109.f; + + // WHEN we calculate altitude from pressure + altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect altitude to be near 3000m + EXPECT_NEAR(altitude, 3000.0f, 0.5f); +} + +TEST(TestAtmosphere, DensityFromPressure) +{ +// GIVEN standard atmosphere at sea level + const float pressureAtSealevel = 101325.f; + const float tempSeaLevel = 15.f; + + // WHEN we calculate density from pressure and temperature + float density = getDensityFromPressureAndTemp(pressureAtSealevel, tempSeaLevel); + + // THEN expect density at sea level in standard atmosphere + EXPECT_NEAR(density, 1.225f, 0.001f); + + // GIVEN standard atmosphere at 3000m + const float pressure = 70109.f; + const float tempAt3000m = -4.5f; + + // WHEN we calculate density from pressure and temperature + density = getDensityFromPressureAndTemp(pressure, tempAt3000m); + + // THEN expect density at 3000m in standard atmosphere + EXPECT_NEAR(density, 0.9091f, 0.001f); +} + +TEST(TestAtmosphere, StandardTemperature) +{ + // GIVEN standard atmosphere at sea level + float altitude = 0.f; + + // WHEN we calculate standard temperature from altitude + float temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at sea level + EXPECT_NEAR(temperature, 15.f, 0.001f); + + // GIVEN standard atmosphere at 3000m + altitude = 3000.f; + + // WHEN we calculate standard temperature from altitude + temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at 3000m + EXPECT_NEAR(temperature, -4.5f, 0.001f); +} \ No newline at end of file diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp index 1136c3f282..89ba247ed2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp @@ -122,7 +122,7 @@ void SdCardChecks::checkAndReport(const Context &context, Report &reporter) events::Log::Error, "Crash dumps present on SD card"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service"); + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD"); } } diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 37fc6c005b..2854fc3b0d 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -130,8 +130,9 @@ list(APPEND EKF_SRCS EKF/vel_pos_fusion.cpp EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp - EKF/zero_gyro_update.cpp - EKF/zero_velocity_update.cpp + + EKF/aid_sources/ZeroGyroUpdate.cpp + EKF/aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) @@ -220,6 +221,7 @@ px4_add_module( #-O0 INCLUDES EKF + EKF/aid_sources ${EKF_GENERATED_DERIVATION_INCLUDE_PATH} PRIORITY "SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 60f9d10c5d..cdb0a56ba5 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -47,8 +47,9 @@ list(APPEND EKF_SRCS vel_pos_fusion.cpp yaw_fusion.cpp zero_innovation_heading_update.cpp - zero_gyro_update.cpp - zero_velocity_update.cpp + + aid_sources/ZeroGyroUpdate.cpp + aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) diff --git a/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp new file mode 100644 index 0000000000..6d6c9e8ec2 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef EKF_ESTIMATOR_AID_SOURCE_HPP +#define EKF_ESTIMATOR_AID_SOURCE_HPP + +#include + +#include + +// forward declarations +class Ekf; + +namespace estimator +{ +struct imuSample; +}; + +class EstimatorAidSource +{ +public: + EstimatorAidSource() = default; + virtual ~EstimatorAidSource() = default; + + virtual bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) = 0; + + virtual void reset() = 0; + +private: + + +}; + +#endif // !EKF_ESTIMATOR_AID_SOURCE_HPP diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp similarity index 70% rename from src/modules/ekf2/EKF/zero_gyro_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp index 571bde209a..ea9d3f11a8 100644 --- a/src/modules/ekf2/EKF/zero_gyro_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp @@ -31,21 +31,25 @@ * ****************************************************************************/ -/** - * @file zero_gyro_update.cpp - * Control function for ekf zero gyro update - */ +#include "ZeroGyroUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) +ZeroGyroUpdate::ZeroGyroUpdate() { - if (!(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias))) { - return; - } + reset(); +} +void ZeroGyroUpdate::reset() +{ + _zgup_delta_ang.setZero(); + _zgup_delta_ang_dt = 0.f; +} + +bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +{ // When at rest, fuse the gyro data as a direct observation of the gyro bias - if (_control_status.flags.vehicle_at_rest) { + if (ekf.control_status_flags().vehicle_at_rest) { // Downsample gyro data to run the fusion at a lower rate _zgup_delta_ang += imu_delayed.delta_ang; _zgup_delta_ang_dt += imu_delayed.delta_ang_dt; @@ -54,38 +58,38 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt; if (zero_gyro_update_data_ready) { + Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; - Vector3f innovation = _state.gyro_bias - gyro_bias; + Vector3f innovation = ekf.state().gyro_bias - gyro_bias; - const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f)); + const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f)); - const Vector3f innov_var = getGyroBiasVariance() + obs_var; + const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var; for (int i = 0; i < 3; i++) { - fuseDeltaAngBias(innovation(i), innov_var(i), i); + Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. + const unsigned state_index = State::gyro_bias.idx + i; + + // calculate kalman gain K = PHS, where S = 1/innovation variance + for (int row = 0; row < State::size; row++) { + K(row) = ekf.stateCovariance(row, state_index) / innov_var(i); + } + + ekf.measurementUpdate(K, innov_var(i), innovation(i)); } // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; + + return true; } - } else if (_control_status_prev.flags.vehicle_at_rest) { + } else if (ekf.control_status_prev_flags().vehicle_at_rest) { // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; } -} - -void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index) -{ - VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + State::gyro_bias.idx; - - // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < State::size; row++) { - K(row) = P(row, state_index) / innov_var; - } - - measurementUpdate(K, innov_var, innov); + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp new file mode 100644 index 0000000000..1ad488e539 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef EKF_ZERO_GYRO_UPDATE_HPP +#define EKF_ZERO_GYRO_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroGyroUpdate : public EstimatorAidSource +{ +public: + ZeroGyroUpdate(); + virtual ~ZeroGyroUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + matrix::Vector3f _zgup_delta_ang{}; + float _zgup_delta_ang_dt{0.f}; +}; + +#endif // !EKF_ZERO_GYRO_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp similarity index 63% rename from src/modules/ekf2/EKF/zero_velocity_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index f78dc28cc2..d091e5aadb 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,37 +31,50 @@ * ****************************************************************************/ -/** - * @file zero_velocity_update.cpp - * Control function for ekf zero velocity update - */ +#include "ZeroVelocityUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroVelocityUpdate() +ZeroVelocityUpdate::ZeroVelocityUpdate() +{ + reset(); +} + +void ZeroVelocityUpdate::reset() +{ + _time_last_zero_velocity_fuse = 0; +} + +bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { // Fuse zero velocity at a limited rate (every 200 milliseconds) - const bool zero_velocity_update_data_ready = isTimedOut(_time_last_zero_velocity_fuse, (uint64_t)2e5); + const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us); if (zero_velocity_update_data_ready) { - const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest - && _control_status_prev.flags.vehicle_at_rest - && (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift + const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest + && ekf.control_status_prev_flags().vehicle_at_rest + && (!ekf.isVerticalVelocityAidingActive() + || !ekf.control_status_flags().tilt_align); // otherwise the filter is "too rigid" to follow a position drift if (continuing_conditions_passing) { - Vector3f vel_obs{0, 0, 0}; - Vector3f innovation = _state.vel - vel_obs; + Vector3f vel_obs{0.f, 0.f, 0.f}; + Vector3f innovation = ekf.state().vel - vel_obs; // Set a low variance initially for faster leveling and higher // later to let the states follow the measurements - const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f); - Vector3f innov_var = getVelocityVariance() + obs_var; + + const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f); + Vector3f innov_var = ekf.getVelocityVariance() + obs_var; for (unsigned i = 0; i < 3; i++) { - fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); + ekf.fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); } - _time_last_zero_velocity_fuse = _time_delayed_us; + _time_last_zero_velocity_fuse = imu_delayed.time_us; + + return true; } } + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp new file mode 100644 index 0000000000..0016c584c6 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef EKF_ZERO_VELOCITY_UPDATE_HPP +#define EKF_ZERO_VELOCITY_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroVelocityUpdate : public EstimatorAidSource +{ +public: + ZeroVelocityUpdate(); + virtual ~ZeroVelocityUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) + +}; + +#endif // !EKF_ZERO_VELOCITY_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 91acf6660b..5c7aa5790b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -145,8 +145,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlZeroInnovationHeadingUpdate(); - controlZeroVelocityUpdate(); - controlZeroGyroUpdate(imu_delayed); + _zero_velocity_update.update(*this, imu_delayed); + + if (_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)) { + _zero_gyro_update.update(*this, imu_delayed); + } // Fake position measurement for constraining drift when no other velocity or position measurements controlFakePosFusion(); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a714481b0e..5f1228d244 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -110,7 +110,6 @@ void Ekf::reset() _time_last_hor_vel_fuse = 0; _time_last_ver_vel_fuse = 0; _time_last_heading_fuse = 0; - _time_last_zero_velocity_fuse = 0; _last_known_pos.setZero(); @@ -175,6 +174,8 @@ void Ekf::reset() #if defined(CONFIG_EKF2_RANGE_FINDER) resetEstimatorAidStatus(_aid_src_rng_hgt); #endif // CONFIG_EKF2_RANGE_FINDER + + _zero_velocity_update.reset(); } bool Ekf::update() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fbe782f22e..4311417d36 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -59,6 +59,9 @@ #include #include +#include "aid_sources/ZeroGyroUpdate.hpp" +#include "aid_sources/ZeroVelocityUpdate.hpp" + enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface @@ -83,6 +86,8 @@ public: static uint8_t getNumberOfStates() { return State::size; } + const StateSample &state() const { return _state; } + #if defined(CONFIG_EKF2_BAROMETER) const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } @@ -245,6 +250,7 @@ public: // get the full covariance matrix const matrix::SquareMatrix &covariances() const { return P; } + float stateCovariance(unsigned r, unsigned c) const { return P(r, c); } // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } @@ -317,10 +323,14 @@ public: #endif } + // fuse single velocity and position measurement + bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); + // gyro bias const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s float getGyroBiasLimit() const { return _params.gyro_bias_lim; } + float getGyroNoise() const { return _params.gyro_noise; } // accel bias const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 @@ -466,6 +476,37 @@ public: const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL + + bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) + { + clearInhibitedStateKalmanGains(K); + + const VectorState KS = K * innovation_variance; + SquareMatrixState KHP; + + for (unsigned row = 0; row < State::size; row++) { + for (unsigned col = 0; col < State::size; col++) { + // Instad of literally computing KHP, use an equvalent + // equation involving less mathematical operations + KHP(row, col) = KS(row) * K(col); + } + } + + const bool is_healthy = checkAndFixCovarianceUpdate(KHP); + + if (is_healthy) { + // apply the covariance corrections + P -= KHP; + + fixCovarianceErrors(true); + + // apply the state corrections + fuse(K, innovation); + } + + return is_healthy; + } + private: // set the internal states and status to their default value @@ -514,7 +555,6 @@ private: uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_heading_fuse{0}; - uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) Vector3f _last_known_pos{}; ///< last known local position vector (m) @@ -524,10 +564,6 @@ private: Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - // zero gyro update - Vector3f _zgup_delta_ang{}; - float _zgup_delta_ang_dt{0.f}; - Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) float _height_rate_lpf{0.0f}; float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) @@ -774,9 +810,6 @@ private: void fuseDrag(const dragSample &drag_sample); #endif // CONFIG_EKF2_DRAG_FUSION - // fuse single velocity and position measurement - bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); - void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var); void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var); @@ -914,36 +947,6 @@ private: #endif // CONFIG_EKF2_WIND } - bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) - { - clearInhibitedStateKalmanGains(K); - - const VectorState KS = K * innovation_variance; - SquareMatrixState KHP; - - for (unsigned row = 0; row < State::size; row++) { - for (unsigned col = 0; col < State::size; col++) { - // Instad of literally computing KHP, use an equvalent - // equation involving less mathematical operations - KHP(row, col) = KS(row) * K(col); - } - } - - const bool is_healthy = checkAndFixCovarianceUpdate(KHP); - - if (is_healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(K, innovation); - } - - return is_healthy; - } - // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP); @@ -1070,10 +1073,6 @@ private: void resetHeightToLastKnown(); void stopFakeHgtFusion(); - void controlZeroVelocityUpdate(); - void controlZeroGyroUpdate(const imuSample &imu_delayed); - void fuseDeltaAngBias(float innov, float innov_var, int obs_index); - void controlZeroInnovationHeadingUpdate(); #if defined(CONFIG_EKF2_AUXVEL) @@ -1239,6 +1238,9 @@ private: // if any of the innovations are rejected, then the overall innovation is rejected status.innovation_rejected = innovation_rejected; } + + ZeroGyroUpdate _zero_gyro_update{}; + ZeroVelocityUpdate _zero_velocity_update{}; }; #endif // !EKF_EKF_H diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f9e577dc06..4eb0009a85 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2189,7 +2189,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) { const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; - if ((timestamp_sample != 0) && (timestamp_sample > _status_optical_flow_pub_last)) { + if ((timestamp_sample != 0) && (timestamp_sample > _optical_flow_vel_pub_last)) { vehicle_optical_flow_vel_s flow_vel{}; flow_vel.timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; @@ -2210,6 +2210,8 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); + + _optical_flow_vel_pub_last = timestamp_sample; } } #endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 4e48b4a85a..88d1e59cbf 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -377,6 +377,7 @@ private: uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; hrt_abstime _status_optical_flow_pub_last{0}; + hrt_abstime _optical_flow_vel_pub_last{0}; perf_counter_t _msg_missed_optical_flow_perf{nullptr}; #endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index c4bf9c42f8..95ee75817d 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1561,6 +1561,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->autocontinue = mission_item->autocontinue; + mavlink_mission_item->mission_type = _mission_type; /* default mappings for generic commands */ if (mission_item->frame == MAV_FRAME_MISSION) { diff --git a/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/vehicle_air_data/CMakeLists.txt index c8457cc2df..909f249a7c 100644 --- a/src/modules/sensors/vehicle_air_data/CMakeLists.txt +++ b/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -40,4 +40,6 @@ target_link_libraries(vehicle_air_data data_validator px4_work_queue sensor_calibration + PUBLIC + atmosphere ) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 472d228c58..4c15c74a2a 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -36,11 +36,14 @@ #include #include #include +#include + namespace sensors { using namespace matrix; +using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; @@ -189,8 +192,9 @@ void VehicleAirData::Run() // pressure corrected with offset (if available) _calibration[uorb_index].SensorCorrectionsUpdate(); const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); + const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; - float data_array[3] {pressure_corrected, report.temperature, PressureToAltitude(pressure_corrected)}; + float data_array[3] {pressure_corrected, report.temperature, getAltitudeFromPressure(pressure_corrected, pressure_sealevel_pa)}; _voter.put(uorb_index, report.timestamp, data_array, report.error_count, _priority[uorb_index]); _timestamp_sample_sum[uorb_index] += report.timestamp_sample; @@ -251,11 +255,11 @@ void VehicleAirData::Run() const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; - float altitude = PressureToAltitude(pressure_pa, temperature); + const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; + const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - float air_density = pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius - - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; @@ -295,32 +299,6 @@ void VehicleAirData::Run() perf_end(_cycle_perf); } -float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) const -{ - // calculate altitude using the hypsometric equation - static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin - static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre - - // current pressure at MSL in kPa (QNH in hPa) - const float p1 = _param_sens_baro_qnh.get() * 0.1f; - - // measured pressure in kPa - const float p = pressure_pa * 0.001f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; - - return altitude; -} - void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us) { // check failover and report (save failover report for a cycle where parameters didn't update) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 390f25767f..fd28de34f8 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -79,8 +79,6 @@ private: bool ParametersUpdate(bool force = false); void UpdateStatus(); - float PressureToAltitude(float pressure_pa, float temperature = 15.f) const; - static constexpr int MAX_SENSOR_COUNT = 4; uORB::Publication _sensors_status_baro_pub{ORB_ID(sensors_status_baro)};