fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 20:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'px4/main' into pr-ekf2_output_predictor_misc

Browse Source
This commit is contained in:
Daniel Agar
2023-10-26 11:16:42 -04:00
parent 303e1e5adb 12b291b82f
commit 3503d8388a
40 changed files with 1087 additions and 561 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+1 -1
View File
@@ -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
Tools/setup/ubuntu.sh
+1 -1
View File
@@ -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
boards/px4/fmu-v6x/init/rc.board_sensors
+22 -22
View File
@@ -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
boards/scumaker/pilotpi/default.px4board
+2
View File
@@ -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
platforms/common/px4_log.cpp
+1 -1
View File
@@ -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);
platforms/common/uORB/Subscription.cpp
+1 -1
View File
@@ -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;
platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c
+5 -3
View File
@@ -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:
src/drivers/osd/msp_osd/msp_osd.cpp
+1 -11
View File
@@ -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);
}
src/drivers/osd/msp_osd/msp_osd.hpp
-2
View File
@@ -43,7 +43,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/log_message.h>
@@ -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)};
src/drivers/osd/msp_osd/uorb_to_msp.cpp
+3 -4
View File
@@ -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 {
src/drivers/osd/msp_osd/uorb_to_msp.hpp
-3
View File
@@ -54,7 +54,6 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/log_message.h>
@@ -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
src/drivers/pca9685_pwm_out/Kconfig
+17 -1
View File
@@ -2,4 +2,20 @@ menuconfig DRIVERS_PCA9685_PWM_OUT
bool "pca9685_pwm_out"
default n
---help---
Enable support for pca9685_pwm_out
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
src/drivers/pca9685_pwm_out/PCA9685.cpp
+126 -163
View File
@@ -32,23 +32,66 @@
****************************************************************************/
#include <px4_log.h>
#include <px4_defines.h>
#include <cmath>
#include "PCA9685.h"
#include <px4_platform_common/sem.hpp>
#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);
}
src/drivers/pca9685_pwm_out/PCA9685.h
+62 -60
View File
@@ -34,10 +34,7 @@
#pragma once
#include <cstdint>
#include <drivers/device/i2c.h>
#include <drivers/drv_pwm_output.h>
namespace drv_pca9685_pwm
{
#include <px4_boardconfig.h>
#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;
};
}
src/drivers/pca9685_pwm_out/main.cpp
+137 -118
View File
@@ -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 <lhf2613@gmail.com>
*
* This file serves as the wrapper layer for PCA9685 driver, working with parameters
* and scheduling stuffs on PX4 side.
*
*/
#include <px4_log.h>
@@ -45,11 +46,10 @@
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/sem.hpp>
#include "PCA9685.h"
#include <px4_platform_common/sem.hpp>
#define PCA9685_DEFAULT_IICBUS 1
#define PCA9685_DEFAULT_ADDRESS (0x40)
@@ -59,26 +59,25 @@ using namespace time_literals;
class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, 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","<addr>","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, &param_schd_rate);
} else {
PX4_ERR("param PCA9685_SCHD_HZ not found");
}
param = param_find("PCA9685_PWM_FREQ");
if (param != PARAM_INVALID) {
param_get(param, &param_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*)&param_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);
}
src/drivers/pca9685_pwm_out/module.yaml
+69 -17
View File
@@ -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
src/lib/CMakeLists.txt
+1
View File
@@ -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)
src/lib/airspeed/airspeed.cpp
+6 -24
View File
@@ -44,6 +44,9 @@
#include <px4_platform_common/defines.h>
#include <lib/geo/geo.h>
#include <lib/atmosphere/atmosphere.h>
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);
src/lib/airspeed/airspeed.h
-1
View File
@@ -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
src/lib/atmosphere/CMakeLists.txt
+37
View File
@@ -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)
src/lib/atmosphere/atmosphere.cpp
+81
View File
@@ -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 <geo/geo.h>
#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;
}
}
src/lib/atmosphere/atmosphere.h
+80
View File
@@ -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_
src/lib/atmosphere/test_atmosphere.cpp
+126
View File
@@ -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 <gtest/gtest.h>
#include <lib/atmosphere/atmosphere.h>
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);
}
src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp
+1 -1
View File
@@ -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");
}
}
src/modules/ekf2/CMakeLists.txt
+4 -2
View File
@@ -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
src/modules/ekf2/EKF/CMakeLists.txt
+3 -2
View File
@@ -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)
src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp
+64
View File
@@ -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 <cstdint>
#include <lib/matrix/matrix/math.hpp>
// 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
src/modules/ekf2/EKF/zero_gyro_update.cpp → src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp
+32 -28
View File
@@ -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<int32_t>(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;
}
src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp
+54
View File
@@ -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
src/modules/ekf2/EKF/zero_velocity_update.cpp → src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp
+30 -17
View File
@@ -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;
}
src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp
+54
View File
@@ -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
src/modules/ekf2/EKF/control.cpp
+5 -2
View File
@@ -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<int32_t>(ImuCtrl::GyroBias)) {
_zero_gyro_update.update(*this, imu_delayed);
}
// Fake position measurement for constraining drift when no other velocity or position measurements
controlFakePosFusion();
src/modules/ekf2/EKF/ekf.cpp
+2 -1
View File
@@ -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()
src/modules/ekf2/EKF/ekf.h
+44 -42
View File
@@ -59,6 +59,9 @@
#include <uORB/topics/estimator_aid_source2d.h>
#include <uORB/topics/estimator_aid_source3d.h>
#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<float, State::size> &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<float, State::size> 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<State::gyro_bias>(); } // 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
src/modules/ekf2/EKF2.cpp
+3 -1
View File
@@ -2189,7 +2189,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
{
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 &timestamp)
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
src/modules/ekf2/EKF2.hpp
+1
View File
@@ -377,6 +377,7 @@ private:
uORB::PublicationMulti<estimator_aid_source2d_s> _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
src/modules/mavlink/mavlink_mission.cpp
+1
View File
@@ -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) {
src/modules/sensors/vehicle_air_data/CMakeLists.txt
+2
View File
@@ -40,4 +40,6 @@ target_link_libraries(vehicle_air_data
data_validator
px4_work_queue
sensor_calibration
PUBLIC
atmosphere
)
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
+8 -30
View File
@@ -36,11 +36,14 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/events.h>
#include <lib/geo/geo.h>
#include <lib/atmosphere/atmosphere.h>
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)
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
-2
View File
@@ -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_s> _sensors_status_baro_pub{ORB_ID(sensors_status_baro)};