dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

create temperature_compensation module 

- this is a new module for temperature compensation that consolidates the functionality previously handled in the sensors module (calculating runtime thermal corrections) and the events module (online thermal calibration)
 - by collecting this functionality into a single module we can optionally disable it on systems where it's not used and save some flash (if disabled at build time) or memory (disabled at run time)
This commit is contained in:
Daniel Agar 2020-01-20 21:42:42 -05:00 committed by GitHub
parent dedb4e8267
commit dc05ceaad2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
87 changed files with 978 additions and 739 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ROMFS/px4fmu_common/init.d/rc.thermal_cal
View File

@ -5,12 +5,37 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
set TEMP_CALIB_ARGS ""
set TEMP_COMP_START ""
if param compare TC_A_ENABLE 1
then
set TEMP_COMP_START "true"
fi
if param compare TC_B_ENABLE 1
then
set TEMP_COMP_START "true"
fi
if param compare TC_G_ENABLE 1
then
set TEMP_COMP_START "true"
fi
if [ "x$TEMP_COMP_START" != "x" ]
then
temperature_compensation start
fi
unset TEMP_COMP_START
#
# Determine if a thermal calibration should be started.
#
set TEMP_CALIB_ARGS ""
if param compare SYS_CAL_ACCEL 1
then
set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -a"
@ -34,7 +59,7 @@ fi
#
if [ "x$TEMP_CALIB_ARGS" != "x" ]
then
send_event temperature_calibration ${TEMP_CALIB_ARGS}
temperature_compensation calibrate ${TEMP_CALIB_ARGS}
fi
unset TEMP_CALIB_ARGS
unset TEMP_CALIB_ARGS

1
boards/aerotenna/ocpoc/default.cmake
View File

@ -59,6 +59,7 @@ px4_add_board(
rc_update
rover_pos_control
sensors
temperature_compensation
sih
#simulator
vmount

1
boards/airmind/mindpx-v2/default.cmake
View File

@ -76,6 +76,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/atlflight/eagle/default.cmake
View File

@ -83,6 +83,7 @@ px4_add_board(
rover_pos_control
sensors
#sih
temperature_compensation
simulator
vmount
vtol_att_control

1
boards/atlflight/eagle/qurt.cmake
View File

@ -68,6 +68,7 @@ px4_add_board(
rc_update
rover_pos_control
sensors
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/atlflight/excelsior/qurt.cmake
View File

@ -68,6 +68,7 @@ px4_add_board(
rc_update
rover_pos_control
sensors
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/auav/x21/default.cmake
View File

@ -76,6 +76,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/av/x-v1/default.cmake
View File

@ -76,6 +76,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/beaglebone/blue/default.cmake
View File

@ -53,6 +53,7 @@ px4_add_board(
sensors
sih
#simulator
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/bitcraze/crazyflie/default.cmake
View File

@ -32,6 +32,7 @@ px4_add_board(
navigator
rc_update
sensors
#temperature_compensation
SYSTEMCMDS
bl_update
config

1
boards/emlid/navio2/default.cmake
View File

@ -55,6 +55,7 @@ px4_add_board(
sensors
sih
#simulator
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/holybro/durandal-v1/default.cmake
View File

@ -81,6 +81,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/holybro/durandal-v1/stackcheck.cmake
View File

@ -82,6 +82,7 @@ px4_add_board(
#rover_pos_control
sensors
#sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/holybro/kakutef7/default.cmake
View File

@ -46,6 +46,7 @@ px4_add_board(
navigator
rc_update
sensors
#temperature_compensation
SYSTEMCMDS
bl_update
config

1
boards/intel/aerofc-v1/default.cmake
View File

@ -56,6 +56,7 @@ px4_add_board(
#rover_pos_control
sensors
#sih
#temperature_compensation
vmount
#vtol_att_control
SYSTEMCMDS

1
boards/intel/aerofc-v1/rtps.cmake
View File

@ -56,6 +56,7 @@ px4_add_board(
#rover_pos_control
sensors
#sih
#temperature_compensation
vmount
#vtol_att_control
SYSTEMCMDS

1
boards/modalai/fc-v1/default.cmake
View File

@ -74,6 +74,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/mro/ctrl-zero-f7/default.cmake
View File

@ -80,6 +80,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/nxp/fmuk66-v3/default.cmake
View File

@ -78,6 +78,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/omnibus/f4sd/default.cmake
View File

@ -64,6 +64,7 @@ px4_add_board(
#rover_pos_control
sensors
#sih
#temperature_compensation
#vmount
#vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v2/default.cmake
View File

@ -87,6 +87,7 @@ px4_add_board(
#rover_pos_control
sensors
#sih
#temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v2/fixedwing.cmake
View File

@ -56,6 +56,7 @@ px4_add_board(
navigator
rc_update
sensors
temperature_compensation
vmount
SYSTEMCMDS
#bl_update

1
boards/px4/fmu-v2/lpe.cmake
View File

@ -81,6 +81,7 @@ px4_add_board(
battery_status
rc_update
sensors
temperature_compensation
vmount
#vtol_att_control
#airspeed_selector

2
boards/px4/fmu-v2/multicopter.cmake
View File

@ -54,6 +54,8 @@ px4_add_board(
navigator
rc_update
sensors
sih
#temperature_compensation
vmount
SYSTEMCMDS
#bl_update

1
boards/px4/fmu-v2/rover.cmake
View File

@ -50,6 +50,7 @@ px4_add_board(
battery_status
rc_update
sensors
temperature_compensation
vmount
SYSTEMCMDS

1
boards/px4/fmu-v2/test.cmake
View File

@ -86,6 +86,7 @@ px4_add_board(
#rover_pos_control
sensors
#sih
#temperature_compensation
#vmount
#vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v3/default.cmake
View File

@ -86,6 +86,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v3/rtps.cmake
View File

@ -86,6 +86,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v3/stackcheck.cmake
View File

@ -81,6 +81,7 @@ px4_add_board(
rc_update
sensors
sih
temperature_compensation
vmount
vtol_att_control
airspeed_selector

1
boards/px4/fmu-v4/default.cmake
View File

@ -79,6 +79,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v4/rtps.cmake
View File

@ -77,6 +77,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v4/stackcheck.cmake
View File

@ -77,6 +77,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v4pro/default.cmake
View File

@ -81,6 +81,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v4pro/rtps.cmake
View File

@ -78,6 +78,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

24
boards/px4/fmu-v5/critmonitor.cmake
View File

@ -10,13 +10,11 @@ px4_add_board(
IO px4_io-v2_default
TESTING
UAVCAN_INTERFACES 2
SERIAL_PORTS
GPS1:/dev/ttyS0
TEL1:/dev/ttyS1
TEL2:/dev/ttyS2
TEL4:/dev/ttyS3
DRIVERS
adc
barometer # all available barometer drivers
@ -25,14 +23,15 @@ px4_add_board(
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers
dshot
gps
#heater
imu/adis16448
imu/adis16497
#imu # all available imu drivers
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/mpu6000
imu/mpu9250
irlock
lights/blinkm
lights/rgbled
@ -41,6 +40,7 @@ px4_add_board(
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
power_monitor/ina226
#protocol_splitter
@ -56,9 +56,10 @@ px4_add_board(
test_ppm
tone_alarm
uavcan
MODULES
airspeed_selector
attitude_estimator_q
battery_status
camera_feedback
commander
dataman
@ -66,7 +67,6 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
rover_pos_control
land_detector
landing_target_estimator
load_mon
@ -74,17 +74,17 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_rate_control
mc_pos_control
mc_rate_control
#micrortps_bridge
navigator
battery_status
rc_update
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
airspeed_selector
SYSTEMCMDS
bl_update
config
@ -113,7 +113,6 @@ px4_add_board(
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge
fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
@ -124,5 +123,4 @@ px4_add_board(
px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
rover_steering_control # Rover example app
uuv_example_app
)

1
boards/px4/fmu-v5/default.cmake
View File

@ -82,6 +82,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v5/fixedwing.cmake
View File

@ -59,6 +59,7 @@ px4_add_board(
navigator
rc_update
sensors
temperature_compensation
vmount
SYSTEMCMDS
bl_update

24
boards/px4/fmu-v5/irqmonitor.cmake
View File

@ -10,13 +10,11 @@ px4_add_board(
IO px4_io-v2_default
TESTING
UAVCAN_INTERFACES 2
SERIAL_PORTS
GPS1:/dev/ttyS0
TEL1:/dev/ttyS1
TEL2:/dev/ttyS2
TEL4:/dev/ttyS3
DRIVERS
adc
barometer # all available barometer drivers
@ -25,14 +23,15 @@ px4_add_board(
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers
dshot
gps
#heater
imu/adis16448
imu/adis16497
#imu # all available imu drivers
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/mpu6000
imu/mpu9250
irlock
lights/blinkm
lights/rgbled
@ -41,6 +40,7 @@ px4_add_board(
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
power_monitor/ina226
#protocol_splitter
@ -56,9 +56,10 @@ px4_add_board(
test_ppm
tone_alarm
uavcan
MODULES
airspeed_selector
attitude_estimator_q
battery_status
camera_feedback
commander
dataman
@ -66,7 +67,6 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
rover_pos_control
land_detector
landing_target_estimator
load_mon
@ -74,17 +74,17 @@ px4_add_board(
logger
mavlink
mc_att_control
mc_rate_control
mc_pos_control
mc_rate_control
#micrortps_bridge
navigator
battery_status
rc_update
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
airspeed_selector
SYSTEMCMDS
bl_update
config
@ -113,7 +113,6 @@ px4_add_board(
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge
fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
@ -124,5 +123,4 @@ px4_add_board(
px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
rover_steering_control # Rover example app
uuv_example_app
)

1
boards/px4/fmu-v5/multicopter.cmake
View File

@ -69,6 +69,7 @@ px4_add_board(
rc_update
sensors
sih
temperature_compensation
vmount
SYSTEMCMDS
bl_update

1
boards/px4/fmu-v5/rover.cmake
View File

@ -59,6 +59,7 @@ px4_add_board(
rc_update
rover_pos_control
sensors
temperature_compensation
vmount
SYSTEMCMDS
bl_update

1
boards/px4/fmu-v5/rtps.cmake
View File

@ -82,6 +82,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -83,6 +83,7 @@ px4_add_board(
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

2
boards/px4/fmu-v5x/default.cmake
View File

@ -76,11 +76,13 @@ px4_add_board(
mc_att_control
mc_pos_control
mc_rate_control
#micrortps_bridge
navigator
rc_update
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/raspberrypi/default.cmake
View File

@ -51,6 +51,7 @@ px4_add_board(
rc_update
rover_pos_control
sensors
temperature_compensation
sih
#simulator
vmount

1
boards/px4/sitl/default.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
sensors
#sih
simulator
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/sitl/rtps.cmake
View File

@ -47,6 +47,7 @@ px4_add_board(
sensors
#sih
simulator
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/sitl/test.cmake
View File

@ -45,6 +45,7 @@ px4_add_board(
sensors
#sih
simulator
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS

1
boards/uvify/core/default.cmake
View File

@ -57,6 +57,7 @@ px4_add_board(
rc_update
sensors
sih
temperature_compensation
vmount
SYSTEMCMDS
bl_update

2
msg/sensor_accel_integrated.msg
View File

@ -3,8 +3,6 @@ uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 temperature # temperature in degrees celsius
uint64 error_count
float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt)

4
msg/sensor_correction.msg
View File

@ -46,10 +46,6 @@ float32 baro_scale_1 # barometric pressure scale factors in the sensor frame
float32 baro_offset_2 # barometric pressure offsets in the sensor frame in m/s/s
float32 baro_scale_2 # barometric pressure scale factors in the sensor frame
uint8 selected_gyro_instance # gyro uORB topic instance for the voted sensor
uint8 selected_accel_instance # accelerometer uORB topic instance for the voted sensor
uint8 selected_baro_instance # barometric pressure uORB topic instance for the voted sensor
# Mapping from uORB index to parameter index for each sensor type. For example accel_mapping[1] contains the
# compensation parameter system index for the sensor_accel uORB index 1 data.
uint8[3] gyro_mapping

2
msg/sensor_gyro_integrated.msg
View File

@ -3,8 +3,6 @@ uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 temperature # temperature in degrees celsius
uint64 error_count
float32[3] delta_angle # delta angle in the NED board axis in rad/s over the integration time frame (dt)

7
platforms/common/px4_work_queue/WorkItem.cpp
View File

@ -100,7 +100,7 @@ WorkItem::average_rate() const
{
const float rate = _run_count / elapsed_time();
if (PX4_ISFINITE(rate)) {
if ((_run_count > 0) && PX4_ISFINITE(rate)) {
return rate;
}
@ -110,9 +110,10 @@ WorkItem::average_rate() const
float
WorkItem::average_interval() const
{
const float interval = 1000000.0f / average_rate();
const float rate = average_rate();
const float interval = 1000000.0f / rate;
if (PX4_ISFINITE(interval)) {
if ((rate > 0.0f) && PX4_ISFINITE(interval)) {
return interval;
}

2
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -168,7 +168,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
report.timestamp_sample = _integrator_timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
report.temperature = _temperature;
delta_velocity.copyTo(report.delta_velocity);
report.dt = integral_dt;
report.samples = _integrator_samples;
@ -301,7 +300,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
report.timestamp_sample = _integrator_timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
report.temperature = _temperature;
delta_velocity.copyTo(report.delta_velocity);
report.dt = integrator_dt_us;
report.samples = _integrator_fifo_samples;

2
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -170,7 +170,6 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
report.timestamp_sample = _integrator_timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
report.temperature = _temperature;
delta_angle.copyTo(report.delta_angle);
report.dt = integral_dt;
report.samples = _integrator_samples;
@ -317,7 +316,6 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
report.timestamp_sample = _integrator_timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
report.temperature = _temperature;
delta_angle.copyTo(report.delta_angle);
report.dt = integrator_dt_us;
report.samples = _integrator_fifo_samples;

5
src/modules/events/CMakeLists.txt
View File

@ -40,11 +40,6 @@ px4_add_module(
send_event.cpp
set_leds.cpp
status_display.cpp
subscriber_handler.cpp
temperature_calibration/accel.cpp
temperature_calibration/baro.cpp
temperature_calibration/gyro.cpp
temperature_calibration/task.cpp
DEPENDS
modules__uORB
)

27
src/modules/events/rc_loss_alarm.cpp
View File

@ -52,40 +52,27 @@ namespace events
namespace rc_loss
{
RC_Loss_Alarm::RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler)
: _subscriber_handler(subscriber_handler)
{
}
bool RC_Loss_Alarm::check_for_updates()
{
if (_subscriber_handler.vehicle_status_updated()) {
orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status);
return true;
}
return false;
}
void RC_Loss_Alarm::process()
{
if (!check_for_updates()) {
vehicle_status_s status{};
if (!_vehicle_status_sub.update(&status)) {
return;
}
if (!_was_armed &&
_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
_was_armed = true; // Once true, impossible to go back to false
}
if (!_had_rc && !_vehicle_status.rc_signal_lost) {
if (!_had_rc && !status.rc_signal_lost) {
_had_rc = true;
}
if (_was_armed && _had_rc && _vehicle_status.rc_signal_lost &&
_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
if (_was_armed && _had_rc && status.rc_signal_lost &&
status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
play_tune();
_alarm_playing = true;

10
src/modules/events/rc_loss_alarm.h
View File

@ -39,9 +39,7 @@
#pragma once
#include "subscriber_handler.h"
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
namespace events
@ -53,7 +51,7 @@ class RC_Loss_Alarm
{
public:
RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler);
RC_Loss_Alarm() = default;
/** regularily called to handle state updates */
void process();
@ -71,12 +69,12 @@ private:
/** Publish tune control to interrupt any sound */
void stop_tune();
struct vehicle_status_s _vehicle_status = {};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
bool _was_armed = false;
bool _had_rc = false; // Don't trigger alarm for systems without RC
bool _alarm_playing = false;
orb_advert_t _tune_control_pub = nullptr;
const events::SubscriberHandler &_subscriber_handler;
};
} /* namespace rc_loss */

191
src/modules/events/send_event.cpp
View File

@ -32,7 +32,6 @@
****************************************************************************/
#include "send_event.h"
#include "temperature_calibration/temperature_calibration.h"
#include <math.h>
@ -40,96 +39,66 @@
#include <px4_platform_common/log.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
namespace events
{
struct work_s SendEvent::_work = {};
// Run it at 30 Hz.
const unsigned SEND_EVENT_INTERVAL_US = 33000;
static constexpr uint32_t SEND_EVENT_INTERVAL_US{1_s / 30};
int SendEvent::task_spawn(int argc, char *argv[])
{
int ret = work_queue(LPWORK, &_work, (worker_t)&SendEvent::initialize_trampoline, nullptr, 0);
SendEvent *send_event = new SendEvent();
if (ret < 0) {
return ret;
}
ret = wait_until_running();
if (ret < 0) {
return ret;
if (!send_event) {
PX4_ERR("alloc failed");
return PX4_ERROR;
}
_object.store(send_event);
_task_id = task_id_is_work_queue;
send_event->start();
return 0;
}
SendEvent::SendEvent() : ModuleParams(nullptr)
SendEvent::SendEvent() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
if (_param_ev_tsk_stat_dis.get()) {
_status_display = new status::StatusDisplay(_subscriber_handler);
_status_display = new status::StatusDisplay();
}
if (_param_ev_tsk_rc_loss.get()) {
_rc_loss_alarm = new rc_loss::RC_Loss_Alarm(_subscriber_handler);
_rc_loss_alarm = new rc_loss::RC_Loss_Alarm();
}
}
SendEvent::~SendEvent()
{
ScheduleClear();
delete _status_display;
delete _rc_loss_alarm;
}
int SendEvent::start()
{
if (is_running()) {
return 0;
}
ScheduleOnInterval(SEND_EVENT_INTERVAL_US, 10000);
// Subscribe to the topics.
_subscriber_handler.subscribe();
// Kick off the cycling. We can call it directly because we're already in the work queue context.
cycle();
return 0;
return PX4_OK;
}
void SendEvent::initialize_trampoline(void *arg)
{
SendEvent *send_event = new SendEvent();
if (!send_event) {
PX4_ERR("alloc failed");
return;
}
send_event->start();
_object.store(send_event);
}
void SendEvent::cycle_trampoline(void *arg)
{
SendEvent *obj = static_cast<SendEvent *>(arg);
obj->cycle();
}
void SendEvent::cycle()
void SendEvent::Run()
{
if (should_exit()) {
_subscriber_handler.unsubscribe();
exit_and_cleanup();
return;
}
_subscriber_handler.check_for_updates();
process_commands();
if (_status_display != nullptr) {
@ -139,52 +108,12 @@ void SendEvent::cycle()
if (_rc_loss_alarm != nullptr) {
_rc_loss_alarm->process();
}
work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this,
USEC2TICK(SEND_EVENT_INTERVAL_US));
}
void SendEvent::process_commands()
{
if (!_subscriber_handler.vehicle_command_updated()) {
return;
}
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _subscriber_handler.get_vehicle_command_sub(), &cmd);
bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false;
switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
gyro = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
accel = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
baro = true;
got_temperature_calibration_command = true;
}
if (got_temperature_calibration_command) {
if (run_temperature_calibration(accel, baro, gyro) == 0) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
}
break;
}
// TODO: do something with vehicle commands
// TODO: what is this modules purpose?
}
void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
@ -201,68 +130,6 @@ void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
command_ack_pub.publish(command_ack);
}
int SendEvent::custom_command(int argc, char *argv[])
{
if (!strcmp(argv[0], "temperature_calibration")) {
if (!is_running()) {
PX4_ERR("background task not running");
return -1;
}
bool gyro_calib = false, accel_calib = false, baro_calib = false;
bool calib_all = true;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'a':
accel_calib = true;
calib_all = false;
break;
case 'b':
baro_calib = true;
calib_all = false;
break;
case 'g':
gyro_calib = true;
calib_all = false;
break;
default:
print_usage("unrecognized flag");
return 1;
}
}
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = (float)((gyro_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.param2 = NAN;
vcmd.param3 = NAN;
vcmd.param4 = NAN;
vcmd.param5 = ((accel_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN);
vcmd.param6 = (double)NAN;
vcmd.param7 = (float)((baro_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
} else {
print_usage("unrecognized command");
}
return 0;
}
int SendEvent::print_usage(const char *reason)
{
if (reason) {
@ -273,22 +140,26 @@ int SendEvent::print_usage(const char *reason)
R"DESCR_STR(
### Description
Background process running periodically on the LP work queue to perform housekeeping tasks.
It is currently only responsible for temperature calibration and tone alarm on RC Loss.
It is currently only responsible for tone alarm on RC Loss.
The tasks can be started via CLI or uORB topics (vehicle_command from MAVLink, etc.).
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("send_event", "system");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
PRINT_MODULE_USAGE_COMMAND_DESCR("temperature_calibration", "Run temperature calibration process");
PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true);
PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true);
PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int SendEvent::custom_command(int argc, char *argv[])
{
// TODO: what is my purpose?
print_usage("unrecognized command");
return 0;
}
int send_event_main(int argc, char *argv[])
{
return SendEvent::main(argc, argv);

25
src/modules/events/send_event.h
View File

@ -33,11 +33,10 @@
#pragma once
#include "subscriber_handler.h"
#include "status_display.h"
#include "rc_loss_alarm.h"
#include <px4_platform_common/workqueue.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationQueued.hpp>
@ -50,7 +49,7 @@ namespace events
extern "C" __EXPORT int send_event_main(int argc, char *argv[]);
/** @class SendEvent The SendEvent class manages the RC loss audible alarm, LED status display, and thermal calibration. */
class SendEvent : public ModuleBase<SendEvent>, public ModuleParams
class SendEvent : public ModuleBase<SendEvent>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@ -93,22 +92,10 @@ private:
*/
void answer_command(const vehicle_command_s &cmd, unsigned result);
/**
* @brief Process cycle trampoline for the work queue.
* @param arg Pointer to the task startup arguments.
*/
static void cycle_trampoline(void *arg);
/**
* @brief Calls process_commands() and schedules the next cycle.
*/
void cycle();
/**
* @brief Trampoline for initialisation.
* @param arg Pointer to the task startup arguments.
*/
static void initialize_trampoline(void *arg);
void Run() override;
/**
* @brief Checks for new commands and processes them.
@ -121,11 +108,7 @@ private:
*/
int start();
/** @struct _work The work queue struct. */
static struct work_s _work;
/** @var _subscriber_handler The uORB subscriber handler. */
SubscriberHandler _subscriber_handler;
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
/** @var _status_display Pointer to the status display object. */
status::StatusDisplay *_status_display = nullptr;

15
src/modules/events/status_display.cpp
View File

@ -48,8 +48,7 @@ namespace events
namespace status
{
StatusDisplay::StatusDisplay(const events::SubscriberHandler &subscriber_handler)
: _subscriber_handler(subscriber_handler)
StatusDisplay::StatusDisplay()
{
// set the base color
_led_control.priority = 0;
@ -66,23 +65,19 @@ bool StatusDisplay::check_for_updates()
{
bool got_updates = false;
if (_subscriber_handler.battery_status_updated()) {
orb_copy(ORB_ID(battery_status), _subscriber_handler.get_battery_status_sub(), &_battery_status);
if (_battery_status_sub.update()) {
got_updates = true;
}
if (_subscriber_handler.cpuload_updated()) {
orb_copy(ORB_ID(cpuload), _subscriber_handler.get_cpuload_sub(), &_cpu_load);
if (_cpu_load_sub.update()) {
got_updates = true;
}
if (_subscriber_handler.vehicle_status_flags_updated()) {
orb_copy(ORB_ID(vehicle_status_flags), _subscriber_handler.get_vehicle_status_flags_sub(), &_vehicle_status_flags);
if (_vehicle_status_flags_sub.update()) {
got_updates = true;
}
if (_subscriber_handler.vehicle_status_updated()) {
orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status);
if (_vehicle_status_sub.update()) {
got_updates = true;
}

17
src/modules/events/status_display.h
View File

@ -40,14 +40,14 @@
#pragma once
#include "subscriber_handler.h"
#include <drivers/drv_hrt.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
@ -60,7 +60,7 @@ class StatusDisplay
{
public:
StatusDisplay(const events::SubscriberHandler &subscriber_handler);
StatusDisplay();
/** regularily called to handle state updates */
void process();
@ -81,11 +81,11 @@ protected:
void publish();
// TODO: review if there is a better variant that allocates this in the memory
struct battery_status_s _battery_status = {};
struct cpuload_s _cpu_load = {};
struct vehicle_status_s _vehicle_status = {};
struct vehicle_status_flags_s _vehicle_status_flags = {};
struct vehicle_attitude_s _vehicle_attitude = {};
uORB::SubscriptionData<battery_status_s> _battery_status_sub{ORB_ID(battery_status)};
uORB::SubscriptionData<cpuload_s> _cpu_load_sub{ORB_ID(cpuload)};
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionData<vehicle_status_flags_s> _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
uORB::SubscriptionData<vehicle_attitude_s> _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
struct led_control_s _led_control = {};
@ -99,7 +99,6 @@ private:
uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
const events::SubscriberHandler &_subscriber_handler;
};
} /* namespace status */

102
src/modules/events/subscriber_handler.cpp
View File

@ -1,102 +0,0 @@
#include "subscriber_handler.h"
#include <px4_platform_common/log.h>
namespace events
{
void SubscriberHandler::subscribe()
{
if (_battery_status_sub < 0) {
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
}
if (_cpuload_sub < 0) {
_cpuload_sub = orb_subscribe(ORB_ID(cpuload));
}
if (_vehicle_command_sub < 0) {
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
}
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
if (_vehicle_status_flags_sub < 0) {
_vehicle_status_flags_sub = orb_subscribe(ORB_ID(vehicle_status_flags));
}
}
void SubscriberHandler::unsubscribe()
{
if (_battery_status_sub >= 0) {
orb_unsubscribe(_battery_status_sub);
_battery_status_sub = -1;
}
if (_cpuload_sub >= 0) {
orb_unsubscribe(_cpuload_sub);
_cpuload_sub = -1;
}
if (_vehicle_command_sub >= 0) {
orb_unsubscribe(_vehicle_command_sub);
_vehicle_command_sub = -1;
}
if (_vehicle_status_sub >= 0) {
orb_unsubscribe(_vehicle_status_sub);
_vehicle_status_sub = -1;
}
if (_vehicle_status_flags_sub >= 0) {
orb_unsubscribe(_vehicle_status_flags_sub);
_vehicle_status_flags_sub = -1;
}
}
void SubscriberHandler::check_for_updates()
{
bool updated;
_update_bitfield = 0;
orb_check(_vehicle_command_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::VehicleCommand;
}
updated = false;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::VehicleStatus;
}
updated = false;
orb_check(_vehicle_status_flags_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::VehicleStatusFlags;
}
updated = false;
orb_check(_battery_status_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::BatteryStatus;
}
updated = false;
orb_check(_cpuload_sub, &updated);
if (updated) {
_update_bitfield |= (uint32_t)StatusMask::CpuLoad;
}
}
} /* namespace events */

57
src/modules/events/subscriber_handler.h
View File

@ -1,57 +0,0 @@
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_attitude.h>
namespace events
{
/**
* @class SubscriberHandler
* Contains a list of uORB subscriptions and maintains their update state.
*/
class SubscriberHandler
{
public:
void subscribe();
void unsubscribe();
void check_for_updates();
int get_battery_status_sub() const { return _battery_status_sub; }
int get_cpuload_sub() const { return _cpuload_sub; }
int get_vehicle_command_sub() const { return _vehicle_command_sub; }
int get_vehicle_status_sub() const { return _vehicle_status_sub; }
int get_vehicle_status_flags_sub() const { return _vehicle_status_flags_sub; }
/* update checking methods */
bool battery_status_updated() const { return _update_bitfield & (uint32_t)StatusMask::BatteryStatus; }
bool cpuload_updated() const { return _update_bitfield & (uint32_t)StatusMask::CpuLoad; }
bool vehicle_command_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleCommand; }
bool vehicle_status_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleStatus; }
bool vehicle_status_flags_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleStatusFlags; }
private:
enum class StatusMask : uint32_t {
VehicleCommand = (0x01 << 0),
VehicleStatus = (0x01 << 1),
VehicleStatusFlags = (0x01 << 2),
BatteryStatus = (0x01 << 3),
CpuLoad = (0x01 << 4),
};
int _battery_status_sub = -1;
int _cpuload_sub = -1;
int _vehicle_command_sub = -1;
int _vehicle_status_sub = -1;
int _vehicle_status_flags_sub = -1;
uint32_t _update_bitfield = 0;
};
} /* events */

2
src/modules/sensors/CMakeLists.txt
View File

@ -42,8 +42,6 @@ px4_add_module(
voted_sensors_update.cpp
sensors.cpp
parameters.cpp
temperature_compensation.cpp
DEPENDS
airspeed
conversion

65
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -37,6 +37,7 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
VehicleAcceleration::VehicleAcceleration() :
ModuleParams(nullptr),
@ -53,11 +54,15 @@ bool VehicleAcceleration::Start()
{
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
// sensor_selection needed to change the active sensor if the primary stops updating
if (!_sensor_selection_sub.registerCallback()) {
PX4_ERR("sensor_selection callback registration failed");
return false;
}
ScheduleNow();
return true;
}
void VehicleAcceleration::Stop()
@ -97,7 +102,7 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) {
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
@ -122,8 +127,8 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
_scale = Vector3f{corrections.accel_scale_2};
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
@ -154,8 +159,11 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
return true;
}
@ -182,13 +190,13 @@ void VehicleAcceleration::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
math::radians(_param_sens_board_x_off.get()),
math::radians(_param_sens_board_y_off.get()),
math::radians(_param_sens_board_z_off.get())));
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
}
@ -204,28 +212,29 @@ void VehicleAcceleration::Run()
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_accel_s sensor_data;
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f accel{(val - _offset).emult(_scale)};
// apply offsets and scale
Vector3f accel{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
accel = _board_rotation * accel;
// rotate corrected measurements from sensor to body frame
accel = _board_rotation * accel;
// correct for in-run bias errors
accel -= _bias;
// correct for in-run bias errors
accel -= _bias;
// publish
vehicle_acceleration_s out;
// publish
vehicle_acceleration_s out;
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(out);
_vehicle_acceleration_pub.publish(out);
}
}
}

2
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View File

@ -96,9 +96,9 @@ private:
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
matrix::Vector3f _bias{0.f, 0.f, 0.f};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};

65
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -37,6 +37,7 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
VehicleAngularVelocity::VehicleAngularVelocity() :
ModuleParams(nullptr),
@ -53,11 +54,15 @@ bool VehicleAngularVelocity::Start()
{
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
// sensor_selection needed to change the active sensor if the primary stops updating
if (!_sensor_selection_sub.registerCallback()) {
PX4_ERR("sensor_selection callback registration failed");
return false;
}
ScheduleNow();
return true;
}
void VehicleAngularVelocity::Stop()
@ -97,7 +102,7 @@ void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_gyro_instance) || force) {
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
@ -122,8 +127,8 @@ void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
_scale = Vector3f{corrections.gyro_scale_2};
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
@ -154,8 +159,11 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
return true;
}
@ -182,13 +190,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
math::radians(_param_sens_board_x_off.get()),
math::radians(_param_sens_board_y_off.get()),
math::radians(_param_sens_board_z_off.get())));
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
}
@ -204,28 +212,29 @@ void VehicleAngularVelocity::Run()
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_gyro_s sensor_data;
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates -= _bias;
// correct for in-run bias errors
rates -= _bias;
// publish
vehicle_angular_velocity_s out;
// publish
vehicle_angular_velocity_s out;
out.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
out.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(out);
_vehicle_angular_velocity_pub.publish(out);
}
}
}

115
src/modules/sensors/voted_sensors_update.cpp
View File

@ -84,7 +84,6 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
initializeSensors();
_corrections_changed = true; //make sure to initially publish the corrections topic
_selection_changed = true;
return 0;
@ -132,76 +131,6 @@ void VotedSensorsUpdate::parametersUpdate()
_mag_rotation[topic_instance] = _board_rotation;
}
/* Load & apply the sensor calibrations.
* IMPORTANT: we assume all sensor drivers are running and published sensor data at this point
*/
/* temperature compensation */
_temperature_compensation.parameters_update(_hil_enabled);
/* gyro */
for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_gyro_integrated_s> report{ORB_ID(sensor_gyro_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "gyro", report.get().device_id,
topic_instance);
_corrections.gyro_mapping[topic_instance] = 0;
_corrections.gyro_device_ids[topic_instance] = 0;
} else {
_corrections.gyro_mapping[topic_instance] = temp;
_corrections.gyro_device_ids[topic_instance] = report.get().device_id;
}
}
/* accel */
for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_accel_integrated_s> report{ORB_ID(sensor_accel_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "accel", report.get().device_id,
topic_instance);
_corrections.accel_mapping[topic_instance] = 0;
_corrections.accel_device_ids[topic_instance] = 0;
} else {
_corrections.accel_mapping[topic_instance] = temp;
_corrections.accel_device_ids[topic_instance] = report.get().device_id;
}
}
/* baro */
for (uint8_t topic_instance = 0; topic_instance < _baro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_baro_s> report{ORB_ID(sensor_baro), topic_instance};
int temp = _temperature_compensation.set_sensor_id_baro(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "baro", report.get().device_id,
topic_instance);
_corrections.baro_mapping[topic_instance] = 0;
_corrections.baro_device_ids[topic_instance] = 0;
} else {
_corrections.baro_mapping[topic_instance] = temp;
_corrections.baro_device_ids[topic_instance] = report.get().device_id;
}
}
/* set offset parameters to new values */
bool failed = false;
@ -601,11 +530,10 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
accel_data(0) = (accel_data(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
accel_data(1) = (accel_data(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
accel_data(2) = (accel_data(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
accel_data = _board_rotation * accel_data;
@ -631,8 +559,6 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
if (best_index != _accel.last_best_vote) {
_accel.last_best_vote = (uint8_t)best_index;
_corrections.selected_accel_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.accel_device_id != _accel_device_id[best_index]) {
@ -685,11 +611,10 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
gyro_rate(0) = (gyro_rate(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
gyro_rate(1) = (gyro_rate(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
gyro_rate(2) = (gyro_rate(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
gyro_rate = _board_rotation * gyro_rate;
@ -716,8 +641,6 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
if (_gyro.last_best_vote != best_index) {
_gyro.last_best_vote = (uint8_t)best_index;
_corrections.selected_gyro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.gyro_device_id != _gyro_device_id[best_index]) {
@ -814,11 +737,8 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
// Convert from millibar to Pa
float corrected_pressure = 100.0f * baro_report.pressure;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
corrected_pressure = (corrected_pressure - *offsets[uorb_index]) * *scales[uorb_index];
// First publication with data
if (_baro.priority[uorb_index] == 0) {
@ -850,8 +770,6 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
_corrections.selected_baro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.baro_device_id != _baro_device_id[best_index]) {
@ -1001,27 +919,18 @@ void VotedSensorsUpdate::printStatus()
_mag.voter.print();
PX4_INFO("baro status:");
_baro.voter.print();
_temperature_compensation.print_status();
}
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata,
vehicle_magnetometer_s &magnetometer)
{
_corrections_sub.update(&_corrections);
accelPoll(raw);
gyroPoll(raw);
magPoll(magnetometer);
baroPoll(airdata);
// publish sensor corrections if necessary
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
_sensor_correction_pub.publish(_corrections);
_corrections_changed = false;
}
// publish sensor selection if changed
if (_selection_changed) {
_selection.timestamp = hrt_absolute_time();

15
src/modules/sensors/voted_sensors_update.h
View File

@ -48,12 +48,14 @@
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/ecl/validation/data_validator.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
@ -64,7 +66,6 @@
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
#include "temperature_compensation.h"
#include "common.h"
namespace sensors
@ -218,8 +219,11 @@ private:
orb_advert_t _mavlink_log_pub{nullptr};
uORB::Publication<sensor_correction_s> _sensor_correction_pub{ORB_ID(sensor_correction)}; /**< handle to the sensor correction uORB topic */
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */
/* sensor thermal compensation */
uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)};
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
@ -231,7 +235,6 @@ private:
const Parameters &_parameters;
const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */
bool _corrections_changed{false};
bool _selection_changed{false}; /**< true when a sensor selection has changed and not been published */
float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */
@ -248,10 +251,6 @@ private:
sensor_correction_s _corrections {}; /**< struct containing the sensor corrections to be published to the uORB */
sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */
subsystem_info_s _info {}; /**< subsystem info publication */
TemperatureCompensation _temperature_compensation{}; /**< sensor thermal compensation */
uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */
};
} /* namespace sensors */

46
src/modules/temperature_compensation/CMakeLists.txt Normal file
View File

@ -0,0 +1,46 @@
############################################################################
#
# Copyright (c) 2015-2019 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_module(
MODULE modules__temperature_compensation
MAIN temperature_compensation
SRCS
TemperatureCompensationModule.cpp
TemperatureCompensation.cpp
temperature_calibration/accel.cpp
temperature_calibration/baro.cpp
temperature_calibration/gyro.cpp
temperature_calibration/task.cpp
DEPENDS
mathlib
)

71
src/modules/sensors/temperature_compensation.cpp → src/modules/temperature_compensation/TemperatureCompensation.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2020 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
@ -39,17 +39,17 @@
* @author Paul Riseborough <gncsolns@gmail.com>
*/
#include "temperature_compensation.h"
#include "TemperatureCompensation.h"
#include <parameters/param.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
namespace sensors
namespace temperature_compensation
{
int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &parameter_handles)
{
char nbuf[16];
char nbuf[16] {};
int ret = PX4_ERROR;
/* rate gyro calibration parameters */
@ -151,24 +151,17 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
return PX4_OK;
}
int TemperatureCompensation::parameters_update(bool hil_enabled)
int TemperatureCompensation::parameters_update()
{
int ret = 0;
ParameterHandles parameter_handles;
ret = initialize_parameter_handles(parameter_handles);
int ret = initialize_parameter_handles(parameter_handles);
if (ret != 0) {
return ret;
}
/* rate gyro calibration parameters */
if (!hil_enabled) {
param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable);
} else {
_parameters.gyro_tc_enable = 0;
}
param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable);
if (_parameters.gyro_tc_enable == 1) {
for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) {
@ -201,12 +194,7 @@ int TemperatureCompensation::parameters_update(bool hil_enabled)
}
/* accelerometer calibration parameters */
if (!hil_enabled) {
param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable);
} else {
_parameters.accel_tc_enable = 0;
}
param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable);
if (_parameters.accel_tc_enable == 1) {
for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
@ -239,12 +227,7 @@ int TemperatureCompensation::parameters_update(bool hil_enabled)
}
/* barometer calibration parameters */
if (!hil_enabled) {
param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable);
} else {
_parameters.baro_tc_enable = 0;
}
param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable);
if (_parameters.baro_tc_enable == 1) {
for (unsigned j = 0; j < BARO_COUNT_MAX; j++) {
@ -348,7 +331,6 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(const SensorCalData3D &coe
}
return ret;
}
int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance)
@ -392,27 +374,30 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc
return -1;
}
int TemperatureCompensation::apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data,
float temperature, float *offsets, float *scales)
int TemperatureCompensation::update_scales_and_offsets_gyro(int topic_instance, float temperature, float *offsets,
float *scales)
{
// Check if temperature compensation is enabled
if (_parameters.gyro_tc_enable != 1) {
return 0;
}
// Map device ID to uORB topic instance
uint8_t mapping = _gyro_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
// Calculate and update the offsets
calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets);
// get the sensor scale factors and correct the data
// Update the scales
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.gyro_cal_data[mapping].scale[axis_index];
sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index];
}
// Check if temperature delta is large enough to warrant a new publication
if (fabsf(temperature - _gyro_data.last_temperature[topic_instance]) > 1.0f) {
_gyro_data.last_temperature[topic_instance] = temperature;
return 2;
@ -421,27 +406,30 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, matrix::
return 1;
}
int TemperatureCompensation::apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data,
float temperature, float *offsets, float *scales)
int TemperatureCompensation::update_scales_and_offsets_accel(int topic_instance, float temperature, float *offsets,
float *scales)
{
// Check if temperature compensation is enabled
if (_parameters.accel_tc_enable != 1) {
return 0;
}
// Map device ID to uORB topic instance
uint8_t mapping = _accel_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
// Calculate and update the offsets
calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
// get the sensor scale factors and correct the data
// Update the scales
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.accel_cal_data[mapping].scale[axis_index];
sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index];
}
// Check if temperature delta is large enough to warrant a new publication
if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) {
_accel_data.last_temperature[topic_instance] = temperature;
return 2;
@ -450,25 +438,28 @@ int TemperatureCompensation::apply_corrections_accel(int topic_instance, matrix:
return 1;
}
int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
float *offsets, float *scales)
int TemperatureCompensation::update_scales_and_offsets_baro(int topic_instance, float temperature, float *offsets,
float *scales)
{
// Check if temperature compensation is enabled
if (_parameters.baro_tc_enable != 1) {
return 0;
}
// Map device ID to uORB topic instance
uint8_t mapping = _baro_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
// Calculate and update the offsets
calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets);
// get the sensor scale factors and correct the data
// Update the scales
*scales = _parameters.baro_cal_data[mapping].scale;
sensor_data = (sensor_data - *offsets) * *scales;
// Check if temperature delta is large enough to warrant a new publication
if (fabsf(temperature - _baro_data.last_temperature[topic_instance]) > 1.0f) {
_baro_data.last_temperature[topic_instance] = temperature;
return 2;
@ -517,4 +508,4 @@ void TemperatureCompensation::print_status()
}
}
}
} // namespace temperature_compensation

78
src/modules/sensors/temperature_compensation.h → src/modules/temperature_compensation/TemperatureCompensation.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2020 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
@ -46,18 +46,19 @@
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include "common.h"
namespace sensors
namespace temperature_compensation
{
static_assert(GYRO_COUNT_MAX == 3,
"GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t BARO_COUNT_MAX = 3;
static_assert(GYRO_COUNT_MAX == 3, "GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static_assert(ACCEL_COUNT_MAX == 3,
"ACCEL_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static_assert(BARO_COUNT_MAX == 3,
"BARO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static_assert(BARO_COUNT_MAX == 3, "BARO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static constexpr uint8_t SENSOR_COUNT_MAX = 3;
/**
** class TemperatureCompensation
@ -68,7 +69,7 @@ class TemperatureCompensation
public:
/** (re)load the parameters. Make sure to call this on startup as well */
int parameters_update(bool hil_enabled = false);
int parameters_update();
/** supply information which device_id matches a specific uORB topic_instance
* (needed if a system has multiple sensors of the same type)
@ -77,7 +78,6 @@ public:
int set_sensor_id_accel(uint32_t device_id, int topic_instance);
int set_sensor_id_baro(uint32_t device_id, int topic_instance);
/**
* Apply Thermal corrections to gyro (& other) sensor data.
* @param topic_instance uORB topic instance
@ -90,13 +90,9 @@ public:
* 1: corrections applied but no changes to offsets & scales,
* 2: corrections applied and offsets & scales updated
*/
int apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
float *scales);
int apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
float *scales);
int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, float *offsets, float *scales);
int update_scales_and_offsets_gyro(int topic_instance, float temperature, float *offsets, float *scales);
int update_scales_and_offsets_accel(int topic_instance, float temperature, float *offsets, float *scales);
int update_scales_and_offsets_baro(int topic_instance, float temperature, float *offsets, float *scales);
/** output current configuration status to console */
void print_status();
@ -187,22 +183,26 @@ private:
// create a struct containing all thermal calibration parameters
struct Parameters {
int32_t gyro_tc_enable;
SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX];
int32_t accel_tc_enable;
SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX];
int32_t baro_tc_enable;
SensorCalData1D baro_cal_data[BARO_COUNT_MAX];
int32_t gyro_tc_enable{0};
SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX] {};
int32_t accel_tc_enable{0};
SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {};
int32_t baro_tc_enable{0};
SensorCalData1D baro_cal_data[BARO_COUNT_MAX] {};
};
// create a struct containing the handles required to access all calibration parameters
struct ParameterHandles {
param_t gyro_tc_enable;
SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX];
param_t accel_tc_enable;
SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX];
param_t baro_tc_enable;
SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX];
param_t gyro_tc_enable{PARAM_INVALID};
SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX] {};
param_t accel_tc_enable{PARAM_INVALID};
SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {};
param_t baro_tc_enable{PARAM_INVALID};
SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX] {};
};
@ -256,22 +256,30 @@ private:
struct PerSensorData {
PerSensorData()
{
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { device_mapping[i] = 255; last_temperature[i] = -100.0f; }
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
device_mapping[i] = 255;
last_temperature[i] = -100.0f;
}
}
void reset_temperature()
{
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { last_temperature[i] = -100.0f; }
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
last_temperature[i] = -100.0f;
}
}
uint8_t device_mapping[SENSOR_COUNT_MAX]; /// map a topic instance to the parameters index
float last_temperature[SENSOR_COUNT_MAX];
uint8_t device_mapping[SENSOR_COUNT_MAX] {}; /// map a topic instance to the parameters index
float last_temperature[SENSOR_COUNT_MAX] {};
};
PerSensorData _gyro_data;
PerSensorData _accel_data;
PerSensorData _baro_data;
template<typename T>
static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
const T *sensor_cal_data, uint8_t sensor_count_max);

421
src/modules/temperature_compensation/TemperatureCompensationModule.cpp Normal file
View File

@ -0,0 +1,421 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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.
*
****************************************************************************/
#include "TemperatureCompensationModule.h"
#include "temperature_calibration/temperature_calibration.h"
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_command.h>
#include <systemlib/mavlink_log.h>
using namespace temperature_compensation;
using namespace time_literals;
TemperatureCompensationModule::TemperatureCompensationModule() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
_loop_perf(perf_alloc(PC_ELAPSED, "temperature_compensation"))
{
// Initialize the publication variables
for (unsigned i = 0; i < 3; i++) {
_corrections.gyro_scale_0[i] = 1.0f;
_corrections.accel_scale_0[i] = 1.0f;
_corrections.gyro_scale_1[i] = 1.0f;
_corrections.accel_scale_1[i] = 1.0f;
_corrections.gyro_scale_2[i] = 1.0f;
_corrections.accel_scale_2[i] = 1.0f;
}
_corrections.baro_scale_0 = 1.0f;
_corrections.baro_scale_1 = 1.0f;
_corrections.baro_scale_2 = 1.0f;
}
TemperatureCompensationModule::~TemperatureCompensationModule()
{
perf_free(_loop_perf);
}
void TemperatureCompensationModule::parameters_update()
{
_temperature_compensation.parameters_update();
// Gyro
for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
sensor_gyro_s report;
if (_gyro_subs[uorb_index].copy(&report)) {
int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, uorb_index);
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "gyro", report.device_id, uorb_index);
_corrections.gyro_mapping[uorb_index] = 0;
_corrections.gyro_device_ids[uorb_index] = 0;
} else {
_corrections.gyro_mapping[uorb_index] = temp;
_corrections.gyro_device_ids[uorb_index] = report.device_id;
}
}
}
// Accel
for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
sensor_accel_s report;
if (_accel_subs[uorb_index].copy(&report)) {
int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index);
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "accel", report.device_id, uorb_index);
_corrections.accel_mapping[uorb_index] = 0;
_corrections.accel_device_ids[uorb_index] = 0;
} else {
_corrections.accel_mapping[uorb_index] = temp;
_corrections.accel_device_ids[uorb_index] = report.device_id;
}
}
}
// Baro
for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) {
sensor_baro_s report;
if (_baro_subs[uorb_index].copy(&report)) {
int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, uorb_index);
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "baro", report.device_id, uorb_index);
_corrections.baro_mapping[uorb_index] = 0;
_corrections.baro_device_ids[uorb_index] = 0;
} else {
_corrections.baro_mapping[uorb_index] = temp;
_corrections.baro_device_ids[uorb_index] = temp;
}
}
}
}
void TemperatureCompensationModule::accelPoll()
{
float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 };
float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 };
// For each accel instance
for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
sensor_accel_s report;
// Grab temperature from report
if (_accel_subs[uorb_index].update(&report)) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_accel(uorb_index, report.temperature, offsets[uorb_index],
scales[uorb_index]) == 2) {
_corrections.accel_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
void TemperatureCompensationModule::gyroPoll()
{
float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 };
float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 };
// For each gyro instance
for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
sensor_gyro_s report;
// Grab temperature from report
if (_gyro_subs[uorb_index].update(&report)) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index],
scales[uorb_index]) == 2) {
_corrections.gyro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
void TemperatureCompensationModule::baroPoll()
{
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 };
float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 };
// For each baro instance
for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) {
sensor_baro_s report;
// Grab temperature from report
if (_baro_subs[uorb_index].update(&report)) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_baro(uorb_index, report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections.baro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
void TemperatureCompensationModule::Run()
{
perf_begin(_loop_perf);
// Check if user has requested to run the calibration routine
if (_vehicle_command_sub.updated()) {
vehicle_command_s cmd;
if (_vehicle_command_sub.copy(&cmd)) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
bool got_temperature_calibration_command = false;
bool accel = false;
bool baro = false;
bool gyro = false;
if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
gyro = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
accel = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
baro = true;
got_temperature_calibration_command = true;
}
if (got_temperature_calibration_command) {
int ret = run_temperature_calibration(accel, baro, gyro);
// publish ACK
vehicle_command_ack_s command_ack{};
if (ret == 0) {
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
}
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
command_ack_pub.publish(command_ack);
}
}
}
}
// Check if any parameter has changed
if (_params_sub.updated()) {
// Read from param to clear updated flag
parameter_update_s update;
_params_sub.copy(&update);
parameters_update();
}
accelPoll();
gyroPoll();
baroPoll();
// publish sensor corrections if necessary
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
_sensor_correction_pub.publish(_corrections);
_corrections_changed = false;
}
perf_end(_loop_perf);
}
int TemperatureCompensationModule::task_spawn(int argc, char *argv[])
{
TemperatureCompensationModule *instance = new TemperatureCompensationModule();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
bool TemperatureCompensationModule::init()
{
ScheduleOnInterval(1_s);
return true;
}
int TemperatureCompensationModule::custom_command(int argc, char *argv[])
{
if (!strcmp(argv[0], "calibrate")) {
bool accel_calib = false;
bool baro_calib = false;
bool gyro_calib = false;
bool calib_all = true;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'a':
accel_calib = true;
calib_all = false;
break;
case 'b':
baro_calib = true;
calib_all = false;
break;
case 'g':
gyro_calib = true;
calib_all = false;
break;
default:
print_usage("unrecognized flag");
return PX4_ERROR;
}
}
if (!is_running()) {
PX4_WARN("background task not running");
if (task_spawn(0, nullptr) != PX4_OK) {
return PX4_ERROR;
}
}
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = (float)((gyro_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.param2 = NAN;
vcmd.param3 = NAN;
vcmd.param4 = NAN;
vcmd.param5 = ((accel_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN);
vcmd.param6 = (double)NAN;
vcmd.param7 = (float)((baro_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
return PX4_OK;
} else {
print_usage("unrecognized command");
return PX4_ERROR;
}
}
int TemperatureCompensationModule::print_status()
{
_temperature_compensation.print_status();
return PX4_OK;
}
int TemperatureCompensationModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
The temperature compensation module allows all of the gyro(s), accel(s), and baro(s) in the system to be temperature
compensated. The module monitors the data coming from the sensors and updates the associated sensor_thermal_cal topic
whenever a change in temperature is detected. The module can also be configured to perform the coeffecient calculation
routine at next boot, which allows the thermal calibration coeffecients to be calculated while the vehicle undergoes
a temperature cycle.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("temperature_compensation", "system");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module, which monitors the sensors and updates the sensor_thermal_cal topic");
PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run temperature calibration process");
PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true);
PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true);
PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int temperature_compensation_main(int argc, char *argv[])
{
return TemperatureCompensationModule::main(argc, argv);
}

139
src/modules/temperature_compensation/TemperatureCompensationModule.h Normal file
View File

@ -0,0 +1,139 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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.
*
****************************************************************************/
#pragma once
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include "TemperatureCompensation.h"
namespace temperature_compensation
{
class TemperatureCompensationModule : public ModuleBase<TemperatureCompensationModule>, public ModuleParams,
public px4::ScheduledWorkItem
{
public:
TemperatureCompensationModule();
~TemperatureCompensationModule() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static TemperatureCompensationModule *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
void Run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
/**
* Initializes scheduling on work queue.
*/
bool init();
private:
void accelPoll();
void gyroPoll();
void baroPoll();
/**
* call this whenever parameters got updated. Make sure to have initialize_sensors() called at least
* once before calling this.
*/
void parameters_update();
uORB::Subscription _accel_subs[ACCEL_COUNT_MAX] {
{ORB_ID(sensor_accel), 0},
{ORB_ID(sensor_accel), 1},
{ORB_ID(sensor_accel), 2}
};
uORB::Subscription _gyro_subs[GYRO_COUNT_MAX] {
{ORB_ID(sensor_gyro), 0},
{ORB_ID(sensor_gyro), 1},
{ORB_ID(sensor_gyro), 2}
};
uORB::Subscription _baro_subs[BARO_COUNT_MAX] {
{ORB_ID(sensor_baro), 0},
{ORB_ID(sensor_baro), 1},
{ORB_ID(sensor_baro), 2}
};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
perf_counter_t _loop_perf; /**< loop performance counter */
orb_advert_t _mavlink_log_pub{nullptr};
/* sensor thermal compensation */
TemperatureCompensation _temperature_compensation;
sensor_correction_s _corrections{}; /**< struct containing the sensor corrections to be published to the uORB*/
uORB::Publication<sensor_correction_s> _sensor_correction_pub{ORB_ID(sensor_correction)};
bool _corrections_changed{true};
};
} // namespace temperature_compensation

0
src/modules/sensors/temp_comp_params_accel.c → src/modules/temperature_compensation/temp_comp_params_accel.c
View File

0
src/modules/sensors/temp_comp_params_baro.c → src/modules/temperature_compensation/temp_comp_params_baro.c
View File

0
src/modules/sensors/temp_comp_params_gyro.c → src/modules/temperature_compensation/temp_comp_params_gyro.c
View File

3
src/modules/events/temperature_calibration/accel.cpp → src/modules/temperature_compensation/temperature_calibration/accel.cpp
View File

@ -49,8 +49,7 @@ TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_r
float max_start_temperature)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
//init subscriptions
// init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_accel));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {

0
src/modules/events/temperature_calibration/accel.h → src/modules/temperature_compensation/temperature_calibration/accel.h
View File

11
src/modules/events/temperature_calibration/baro.cpp → src/modules/temperature_compensation/temperature_calibration/baro.cpp
View File

@ -49,8 +49,7 @@ TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_ris
float max_start_temperature)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
//init subscriptions
// init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_baro));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
@ -71,7 +70,7 @@ TemperatureCalibrationBaro::~TemperatureCalibrationBaro()
void TemperatureCalibrationBaro::reset_calibration()
{
//nothing to do
// nothing to do
}
int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub)
@ -133,7 +132,7 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
// TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
@ -144,7 +143,7 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
// update linear fit matrices
double relative_temperature = (double)data.sensor_sample_filt[1] - (double)data.ref_temp;
data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
@ -173,7 +172,7 @@ int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int
return 0;
}
double res[POLYFIT_ORDER + 1] = {};
double res[POLYFIT_ORDER + 1] {};
data.P[0].fit(res);
res[POLYFIT_ORDER] =
0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero

0
src/modules/events/temperature_calibration/baro.h → src/modules/temperature_compensation/temperature_calibration/baro.h
View File

6
src/modules/events/temperature_calibration/common.h → src/modules/temperature_compensation/temperature_calibration/common.h
View File

@ -62,7 +62,7 @@ public:
: _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature),
_max_start_temperature(max_start_temperature) {}
virtual ~TemperatureCalibrationBase() {}
virtual ~TemperatureCalibrationBase() = default;
/**
* check & update new sensor data.
@ -97,11 +97,9 @@ protected:
float _max_start_temperature; ///< maximum temperature above which the process does not start and an error is declared
};
int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value)
{
char param_str[30];
char param_str[30] {};
(void)sprintf(param_str, format_str, index);
int result = param_set_no_notification(param_find(param_str), value);

10
src/modules/events/temperature_calibration/gyro.cpp → src/modules/temperature_compensation/temperature_calibration/gyro.cpp
View File

@ -53,7 +53,6 @@ TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_ris
}
_num_sensor_instances = num_gyros;
}
void TemperatureCalibrationGyro::reset_calibration()
@ -79,7 +78,7 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int
return finished ? 0 : 1;
}
sensor_gyro_s gyro_data;
sensor_gyro_s gyro_data{};
orb_copy(ORB_ID(sensor_gyro), sensor_sub, &gyro_data);
if (finished) {
@ -128,7 +127,7 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
// TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
@ -171,7 +170,7 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int
return 0;
}
double res[3][4] = {};
double res[3][4] {};
data.P[0].fit(res[0]);
PX4_INFO("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
(double)res[0][2],
@ -186,7 +185,7 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int
(double)res[2][3]);
data.tempcal_complete = true;
char str[30];
char str[30] {};
float param = 0.0f;
int result = PX4_OK;
@ -207,5 +206,6 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int
set_parameter("TC_G%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp);
return 0;
}

0
src/modules/events/temperature_calibration/gyro.h → src/modules/temperature_compensation/temperature_calibration/gyro.h
View File

10
src/modules/events/temperature_calibration/polyfit.hpp → src/modules/temperature_compensation/temperature_calibration/polyfit.hpp
View File

@ -93,15 +93,9 @@ Author: Siddharth Bharat Purohit
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <time.h>
#include <float.h>
#include <matrix/math.hpp>
#define DEBUG 0

63
src/modules/events/temperature_calibration/task.cpp → src/modules/temperature_compensation/temperature_calibration/task.cpp
View File

@ -42,6 +42,7 @@
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
@ -60,21 +61,14 @@ class TemperatureCalibration;
namespace temperature_calibration
{
TemperatureCalibration *instance = nullptr;
px4::atomic<TemperatureCalibration *> instance{nullptr};
}
class TemperatureCalibration
{
public:
/**
* Constructor
*/
TemperatureCalibration(bool accel, bool baro, bool gyro);
/**
* Destructor
*/
TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) {}
~TemperatureCalibration() = default;
/**
@ -98,21 +92,16 @@ private:
bool _force_task_exit = false;
int _control_task = -1; // task handle for task
bool _accel; ///< enable accel calibration?
bool _baro; ///< enable baro calibration?
bool _gyro; ///< enable gyro calibration?
const bool _accel; ///< enable accel calibration?
const bool _baro; ///< enable baro calibration?
const bool _gyro; ///< enable gyro calibration?
};
TemperatureCalibration::TemperatureCalibration(bool accel, bool baro, bool gyro)
: _accel(accel), _baro(baro), _gyro(gyro)
{
}
void TemperatureCalibration::task_main()
{
// subscribe to all gyro instances
int gyro_sub[SENSOR_COUNT_MAX] = {};
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
int gyro_sub[SENSOR_COUNT_MAX] {-1, -1, -1};
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] {};
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
if (num_gyro > SENSOR_COUNT_MAX) {
@ -136,8 +125,8 @@ void TemperatureCalibration::task_main()
param_get(param_find("SYS_CAL_TMAX"), &max_start_temp);
//init calibrators
TemperatureCalibrationBase *calibrators[3];
bool error_reported[3] = {};
TemperatureCalibrationBase *calibrators[3] {};
bool error_reported[3] {};
int num_calibrators = 0;
if (_accel) {
@ -187,7 +176,7 @@ void TemperatureCalibration::task_main()
hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
// control LED's: blink, then turn solid according to progress
led_control_s led_control = {};
led_control_s led_control{};
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_NORMAL;
led_control.priority = led_control_s::MAX_PRIORITY;
@ -316,20 +305,19 @@ void TemperatureCalibration::task_main()
orb_unsubscribe(gyro_sub[i]);
}
delete temperature_calibration::instance;
temperature_calibration::instance = nullptr;
delete temperature_calibration::instance.load();
temperature_calibration::instance.store(nullptr);
PX4_INFO("Exiting temperature calibration task");
}
int TemperatureCalibration::do_temperature_calibration(int argc, char *argv[])
{
temperature_calibration::instance->task_main();
temperature_calibration::instance.load()->task_main();
return 0;
}
int TemperatureCalibration::start()
{
_control_task = px4_task_spawn_cmd("temperature_calib",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
@ -338,8 +326,8 @@ int TemperatureCalibration::start()
nullptr);
if (_control_task < 0) {
delete temperature_calibration::instance;
temperature_calibration::instance = nullptr;
delete temperature_calibration::instance.load();
temperature_calibration::instance.store(nullptr);
PX4_ERR("start failed");
return -errno;
}
@ -355,13 +343,20 @@ void TemperatureCalibration::publish_led_control(led_control_s &led_control)
int run_temperature_calibration(bool accel, bool baro, bool gyro)
{
PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro);
temperature_calibration::instance = new TemperatureCalibration(accel, baro, gyro);
if (temperature_calibration::instance.load() == nullptr) {
PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro);
temperature_calibration::instance.store(new TemperatureCalibration(accel, baro, gyro));
if (temperature_calibration::instance == nullptr) {
PX4_ERR("alloc failed");
return 1;
if (temperature_calibration::instance.load() == nullptr) {
PX4_ERR("alloc failed");
return 1;
}
return temperature_calibration::instance.load()->start();
} else {
PX4_WARN("temperature calibration task already running");
}
return temperature_calibration::instance->start();
return PX4_ERROR;
}

1
src/modules/events/temperature_calibration/temperature_calibration.h → src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h
View File

@ -37,4 +37,3 @@
/** start temperature calibration in a new task for one or multiple sensors
* @return 0 on success, <0 error otherwise */
int run_temperature_calibration(bool accel, bool baro, bool gyro);