fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 12:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

create temperature_compensation module

Browse Source 
- 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
87 changed files with 978 additions and 739 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/events/CMakeLists.txt
-5
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
)
src/modules/events/rc_loss_alarm.cpp
+7 -20
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;
src/modules/events/rc_loss_alarm.h
+4 -6
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 */
src/modules/events/send_event.cpp
+31 -160
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);
src/modules/events/send_event.h
+4 -21
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;
src/modules/events/status_display.cpp
+5 -10
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;
}
src/modules/events/status_display.h
+8 -9
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 */
src/modules/events/subscriber_handler.cpp
-102
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 */
src/modules/events/subscriber_handler.h
-57
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 */
src/modules/events/temperature_calibration/accel.cpp
-231
View File
@@ -1,231 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file accel.cpp
* Implementation of the Accel Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Paul Riseborough
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include "accel.h"
#include <uORB/topics/sensor_accel.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature,
float max_start_temperature)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
//init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_accel));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
}
}
TemperatureCalibrationAccel::~TemperatureCalibrationAccel()
{
for (unsigned i = 0; i < _num_sensor_instances; i++) {
orb_unsubscribe(_sensor_subs[i]);
}
}
void TemperatureCalibrationAccel::reset_calibration()
{
/* reset all driver level calibrations */
float offset = 0.0f;
float scale = 1.0f;
for (unsigned s = 0; s < 3; s++) {
set_parameter("CAL_ACC%u_XOFF", s, &offset);
set_parameter("CAL_ACC%u_YOFF", s, &offset);
set_parameter("CAL_ACC%u_ZOFF", s, &offset);
set_parameter("CAL_ACC%u_XSCALE", s, &scale);
set_parameter("CAL_ACC%u_YSCALE", s, &scale);
set_parameter("CAL_ACC%u_ZSCALE", s, &scale);
}
}
int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_accel_s accel_data;
orb_copy(ORB_ID(sensor_accel), sensor_sub, &accel_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = accel_data.device_id;
data.sensor_sample_filt[0] = accel_data.x;
data.sensor_sample_filt[1] = accel_data.y;
data.sensor_sample_filt[2] = accel_data.z;
data.sensor_sample_filt[3] = accel_data.temperature;
// wait for min start temp to be reached before starting calibration
if (data.sensor_sample_filt[3] < _min_start_temperature) {
return 1;
}
if (!data.cold_soaked) {
// allow time for sensors and filters to settle
if (hrt_absolute_time() > 10E6) {
// If intial temperature exceeds maximum declare an error condition and exit
if (data.sensor_sample_filt[3] > _max_start_temperature) {
return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
} else {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
return 1;
}
} else {
return 1;
}
}
// check if temperature increased
if (data.sensor_sample_filt[3] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[3];
data.hot_soak_sat = 0;
} else {
return 1;
}
//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;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1],
(double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
return 1;
}
int TemperatureCalibrationAccel::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_A_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[3][4] = {};
data.P[0].fit(res[0]);
res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
(double)res[0][2],
(double)res[0][3]);
data.P[1].fit(res[1]);
res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
(double)res[1][2],
(double)res[1][3]);
data.P[2].fit(res[2]);
res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
(double)res[2][2],
(double)res[2][3]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_A%d_ID", sensor_index, &data.device_id);
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp);
return 0;
}
src/modules/events/temperature_calibration/accel.h
-60
View File
@@ -1,60 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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 "common.h"
#include "polyfit.hpp"
class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3>
{
public:
TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature);
virtual ~TemperatureCalibrationAccel();
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
};
src/modules/events/temperature_calibration/baro.cpp
-204
View File
@@ -1,204 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file baro.cpp
* Implementation of the Baro Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Paul Riseborough
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include "baro.h"
#include <uORB/topics/sensor_baro.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature,
float max_start_temperature)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
//init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_baro));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i);
}
}
TemperatureCalibrationBaro::~TemperatureCalibrationBaro()
{
for (unsigned i = 0; i < _num_sensor_instances; i++) {
orb_unsubscribe(_sensor_subs[i]);
}
}
void TemperatureCalibrationBaro::reset_calibration()
{
//nothing to do
}
int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_baro_s baro_data;
orb_copy(ORB_ID(sensor_baro), sensor_sub, &baro_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = baro_data.device_id;
data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa
data.sensor_sample_filt[1] = baro_data.temperature;
// wait for min start temp to be reached before starting calibration
if (data.sensor_sample_filt[1] < _min_start_temperature) {
return 1;
}
if (!data.cold_soaked) {
// allow time for sensors and filters to settle
if (hrt_absolute_time() > 10E6) {
// If intial temperature exceeds maximum declare an error condition and exit
if (data.sensor_sample_filt[1] > _max_start_temperature) {
return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
} else {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[1]; // Record the low temperature
data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise;
return 1;
}
} else {
return 1;
}
}
// check if temperature increased
if (data.sensor_sample_filt[1] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[1];
data.hot_soak_sat = 0;
} else {
return 1;
}
//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;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nBaro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//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]);
return 1;
}
int TemperatureCalibrationBaro::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_B_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_B_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
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
PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0],
(double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_B%d_ID", sensor_index, &data.device_id);
for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
param = (float)res[coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
return 0;
}
src/modules/events/temperature_calibration/baro.h
-63
View File
@@ -1,63 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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 "common.h"
#include "polyfit.hpp"
#define POLYFIT_ORDER 5
class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFIT_ORDER>
{
public:
TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, float max_start_temperature);
virtual ~TemperatureCalibrationBaro();
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
};
src/modules/events/temperature_calibration/common.h
-195
View File
@@ -1,195 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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
#define TC_PRINT_DEBUG 0
#if TC_PRINT_DEBUG
#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define TC_DEBUG(fmt, ...)
#endif
#include <px4_platform_common/log.h>
#include <mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include "polyfit.hpp"
#define SENSOR_COUNT_MAX 3
#define TC_ERROR_INITIAL_TEMP_TOO_HIGH 110 ///< starting temperature was above the configured allowed temperature
#define TC_ERROR_COMMUNICATION 112 ///< no sensors found
/**
* Base class for temperature calibration types with abstract methods (for all different sensor types)
*/
class TemperatureCalibrationBase
{
public:
TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
: _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature),
_max_start_temperature(max_start_temperature) {}
virtual ~TemperatureCalibrationBase() {}
/**
* check & update new sensor data.
* @return progress in range [0, 100], 110 when finished, <0 on error,
* -TC_ERROR_INITIAL_TEMP_TOO_HIGH if starting temperature is too hot
* -TC_ERROR_COMMUNICATION if no sensors found
*/
virtual int update() = 0;
/**
* do final fitting & write the parameters. Call this exactly once after update() returned 110
* @return 0 on success, <0 otherwise
*/
virtual int finish() = 0;
/** reset all driver-level calibration parameters */
virtual void reset_calibration() = 0;
protected:
/**
* set a system parameter (without system notification) and print an error if it fails
* @param format_str for example "CAL_GYRO%u_XOFF"
* @param index which index (will replace %u in format_str)
* @param value
* @return 0 on success
*/
inline int set_parameter(const char *format_str, unsigned index, const void *value);
float _min_temperature_rise; ///< minimum difference in temperature before the process finishes
float _min_start_temperature; ///< minimum temperature before the process starts
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];
(void)sprintf(param_str, format_str, index);
int result = param_set_no_notification(param_find(param_str), value);
if (result != 0) {
PX4_ERR("unable to reset %s (%i)", param_str, result);
}
return result;
}
/**
** class TemperatureCalibrationCommon
* Common base class for all sensor types, contains shared code & data.
*/
template <int Dim, int PolyfitOrder>
class TemperatureCalibrationCommon : public TemperatureCalibrationBase
{
public:
TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
: TemperatureCalibrationBase(min_temperature_rise, min_start_temperature, max_start_temperature) {}
virtual ~TemperatureCalibrationCommon() = default;
/**
* @see TemperatureCalibrationBase::update()
*/
int update()
{
int num_not_complete = 0;
if (_num_sensor_instances == 0) {
return -TC_ERROR_COMMUNICATION;
}
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
int status = update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
if (status < 0) {
return status;
}
num_not_complete += status;
}
if (num_not_complete > 0) {
// calculate progress
float min_diff = _min_temperature_rise;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;
if (cur_diff < min_diff) {
min_diff = cur_diff;
}
}
return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f));
}
return 110;
}
protected:
struct PerSensorData {
float sensor_sample_filt[Dim + 1]; ///< last value is the temperature
polyfitter < PolyfitOrder + 1 > P[Dim];
unsigned hot_soak_sat = 0; /**< counter that increments every time the sensor temperature reduces
from the last reading */
uint32_t device_id = 0; ///< ID for the sensor being calibrated
bool cold_soaked = false; ///< true when the sensor cold soak starting temperature condition had been
/// verified and the starting temperature set
bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase
bool tempcal_complete = false; ///< true when the calibration has been completed
float low_temp = 0.f; ///< low temperature recorded at start of calibration (deg C)
float high_temp = 0.f; ///< highest temperature recorded during calibration (deg C)
float ref_temp = 0.f; /**< calibration reference temperature, nominally in the middle of the
calibration temperature range (deg C) */
};
PerSensorData _data[SENSOR_COUNT_MAX];
/**
* update a single sensor instance
* @return 0 when done, 1 not finished yet, <0 for an error
*/
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0;
unsigned _num_sensor_instances{0};
int _sensor_subs[SENSOR_COUNT_MAX];
};
src/modules/events/temperature_calibration/gyro.cpp
-211
View File
@@ -1,211 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gyro.cpp
* Implementation of the Gyro Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <mathlib/mathlib.h>
#include <uORB/topics/sensor_gyro.h>
#include "gyro.h"
#include <drivers/drv_hrt.h>
TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature,
float max_start_temperature, int gyro_subs[], int num_gyros)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
for (int i = 0; i < num_gyros; ++i) {
_sensor_subs[i] = gyro_subs[i];
}
_num_sensor_instances = num_gyros;
}
void TemperatureCalibrationGyro::reset_calibration()
{
/* reset all driver level calibrations */
float offset = 0.0f;
for (unsigned s = 0; s < 3; s++) {
set_parameter("CAL_GYRO%u_XOFF", s, &offset);
set_parameter("CAL_GYRO%u_YOFF", s, &offset);
set_parameter("CAL_GYRO%u_ZOFF", s, &offset);
}
}
int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_gyro_s gyro_data;
orb_copy(ORB_ID(sensor_gyro), sensor_sub, &gyro_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = gyro_data.device_id;
data.sensor_sample_filt[0] = gyro_data.x;
data.sensor_sample_filt[1] = gyro_data.y;
data.sensor_sample_filt[2] = gyro_data.z;
data.sensor_sample_filt[3] = gyro_data.temperature;
// wait for min start temp to be reached before starting calibration
if (data.sensor_sample_filt[3] < _min_start_temperature) {
return 1;
}
if (!data.cold_soaked) {
// allow time for sensors and filters to settle
if (hrt_absolute_time() > 10E6) {
// If intial temperature exceeds maximum declare an error condition and exit
if (data.sensor_sample_filt[3] > _max_start_temperature) {
return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
} else {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
return 1;
}
} else {
return 1;
}
}
// check if temperature increased
if (data.sensor_sample_filt[3] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[3];
data.hot_soak_sat = 0;
} else {
return 1;
}
//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;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nGyro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1],
(double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
return 1;
}
int TemperatureCalibrationGyro::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_G_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_G_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
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],
(double)res[0][3]);
data.P[1].fit(res[1]);
PX4_INFO("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
(double)res[1][2],
(double)res[1][3]);
data.P[2].fit(res[2]);
PX4_INFO("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
(double)res[2][2],
(double)res[2][3]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_G%d_ID", sensor_index, &data.device_id);
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
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;
}
src/modules/events/temperature_calibration/gyro.h
-61
View File
@@ -1,61 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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 "common.h"
#include "polyfit.hpp"
class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3>
{
public:
TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature,
int gyro_subs[], int num_gyros);
virtual ~TemperatureCalibrationGyro() {}
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
};
src/modules/events/temperature_calibration/polyfit.hpp
-202
View File
@@ -1,202 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015-2016 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.
*
****************************************************************************/
/*
This algorithm performs a curve fit of m x,y data points using a polynomial
equation of the following form:
yi = a0 + a1.xi + a2.xi^2 + a3.xi^3 + .... + an.xi^n + ei , where:
i = [0,m]
xi is the x coordinate (independant variable) of the i'th measurement
yi is the y coordinate (dependant variable) of the i'th measurement
ei is a random fit error being the difference between the i'th y coordinate
and the value predicted by the polynomial.
In vector form this is represented as:
Y = V.A + E , where:
V is Vandermonde matrix in x -> https://en.wikipedia.org/wiki/Vandermonde_matrix
Y is a vector of length m containing the y measurements
E is a vector of length m containing the fit errors for each measurement
Use an Ordinary Least Squares derivation to minimise ∑(i=0..m)ei^2 -> https://en.wikipedia.org/wiki/Ordinary_least_squares
Note: In the wikipedia reference, the X matrix in reference is equivalent to our V matrix and the Beta matrix is equivalent to our A matrix
A = inv(transpose(V)*V)*(transpose(V)*Y)
We can accumulate VTV and VTY recursively as they are of fixed size, where:
VTV = transpose(V)*V =
__ __
| m+1 x0+x1+...+xm x0^2+x1^2+...+xm^3 .......... x0^n+x1^n+...+xm^n |
|x0+x1+...+xm x0^2+x1^2+...+xm^3 x0^3+x1^3+...+xm^3 .......... x0^(n+1)+x1^(n+1)+...+xm^(n+1) |
| . . . . |
| . . . . |
| . . . . |
|x0^n+x1^n+...+xm^n x0^(n+1)+x1^(n+1)+...+xm^(n+1) x0^(n+2)+x1^(n+2)+...+xm^(n+2) .... x0^(2n)+x1^(2n)+...+xm^(2n) |
|__ __|
and VTY = transpose(V)*Y =
__ __
| ∑(i=0..m)yi |
| ∑(i=0..m)yi*xi |
| . |
| . |
| . |
|∑(i=0..m)yi*xi^n|
|__ __|
*/
/*
Polygon linear fit
Author: Siddharth Bharat Purohit
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#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
#if DEBUG
#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define PF_DEBUG(fmt, ...)
#endif
template<int _forder>
class polyfitter
{
public:
polyfitter() {}
void update(double x, double y)
{
update_VTV(x);
update_VTY(x, y);
}
bool fit(double res[])
{
//Do inverse of VTV
matrix::SquareMatrix<double, _forder> IVTV;
IVTV = _VTV.I();
for (int i = 0; i < _forder; i++) {
for (int j = 0; j < _forder; j++) {
PF_DEBUG("%.10f ", (double)IVTV(i, j));
}
PF_DEBUG("\n");
}
for (int i = 0; i < _forder; i++) {
res[i] = 0.0;
for (int j = 0; j < _forder; j++) {
res[i] += IVTV(i, j) * (double)_VTY(j);
}
PF_DEBUG("%.10f ", res[i]);
}
return true;
}
private:
matrix::SquareMatrix<double, _forder> _VTV;
matrix::Vector<double, _forder> _VTY;
void update_VTY(double x, double y)
{
double temp = 1.0;
PF_DEBUG("O %.6f\n", (double)x);
for (int i = _forder - 1; i >= 0; i--) {
_VTY(i) += y * temp;
temp *= x;
PF_DEBUG("%.6f ", (double)_VTY(i));
}
PF_DEBUG("\n");
}
void update_VTV(double x)
{
double temp = 1.0;
int8_t z;
for (int i = 0; i < _forder; i++) {
for (int j = 0; j < _forder; j++) {
PF_DEBUG("%.10f ", (double)_VTV(i, j));
}
PF_DEBUG("\n");
}
for (int i = 2 * _forder - 2; i >= 0; i--) {
if (i < _forder) {
z = 0.0f;
} else {
z = i - _forder + 1;
}
for (int j = i - z; j >= z; j--) {
int row = j;
int col = i - j;
_VTV(row, col) += (double)temp;
}
temp *= x;
}
}
};
src/modules/events/temperature_calibration/task.cpp
-367
View File
@@ -1,367 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file task.cpp
*
* Main task handling the temperature calibration process
*
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_led.h>
#include <unistd.h>
#include "common.h"
#include "temperature_calibration.h"
#include "accel.h"
#include "baro.h"
#include "gyro.h"
class TemperatureCalibration;
namespace temperature_calibration
{
TemperatureCalibration *instance = nullptr;
}
class TemperatureCalibration
{
public:
/**
* Constructor
*/
TemperatureCalibration(bool accel, bool baro, bool gyro);
/**
* Destructor
*/
~TemperatureCalibration() = default;
/**
* Start task.
*
* @return OK on success.
*/
int start();
static int do_temperature_calibration(int argc, char *argv[]);
void task_main();
void exit_task() { _force_task_exit = true; }
private:
void publish_led_control(led_control_s &led_control);
uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
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?
};
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] = {};
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
if (num_gyro > SENSOR_COUNT_MAX) {
num_gyro = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < num_gyro; i++) {
gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
fds[i].fd = gyro_sub[i];
fds[i].events = POLLIN;
}
int32_t min_temp_rise = 24;
param_get(param_find("SYS_CAL_TDEL"), &min_temp_rise);
PX4_INFO("Waiting for %i degrees difference in sensor temperature", min_temp_rise);
int32_t min_start_temp = 5;
param_get(param_find("SYS_CAL_TMIN"), &min_start_temp);
int32_t max_start_temp = 10;
param_get(param_find("SYS_CAL_TMAX"), &max_start_temp);
//init calibrators
TemperatureCalibrationBase *calibrators[3];
bool error_reported[3] = {};
int num_calibrators = 0;
if (_accel) {
calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
if (_baro) {
calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp, max_start_temp);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
if (_gyro) {
calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_start_temp, max_start_temp, gyro_sub,
num_gyro);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
// reset params
for (int i = 0; i < num_calibrators; ++i) {
calibrators[i]->reset_calibration();
}
// make sure the system updates the changed parameters
param_notify_changes();
px4_usleep(300000); // wait a bit for the system to apply the parameters
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.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_NORMAL;
led_control.priority = led_control_s::MAX_PRIORITY;
led_control.color = led_control_s::COLOR_YELLOW;
led_control.num_blinks = 0;
publish_led_control(led_control);
int leds_completed = 0;
bool abort_calibration = false;
while (!_force_task_exit) {
/* we poll on the gyro(s), since this is the sensor with the highest update rate.
* Each individual sensor will then check on its own if there's new data.
*/
int ret = px4_poll(fds, num_gyro, 1000);
if (ret < 0) {
// Poll error, sleep and try again
px4_usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout or no new data, do nothing
continue;
}
//if gyro is not enabled: we must do an orb_copy here, so that poll() does not immediately return again
if (!_gyro) {
sensor_gyro_s gyro_data;
for (unsigned i = 0; i < num_gyro; ++i) {
orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
}
}
int min_progress = 110;
for (int i = 0; i < num_calibrators; ++i) {
ret = calibrators[i]->update();
if (ret == -TC_ERROR_COMMUNICATION) {
abort_calibration = true;
PX4_ERR("Calibration won't start - sensor bad or communication error");
_force_task_exit = true;
break;
} else if (ret == -TC_ERROR_INITIAL_TEMP_TOO_HIGH) {
abort_calibration = true;
PX4_ERR("Calibration won't start - sensor temperature too high");
_force_task_exit = true;
break;
} else if (ret < 0 && !error_reported[i]) {
// temperature has decreased so calibration is not being updated
error_reported[i] = true;
PX4_ERR("Calibration update step failed (%i)", ret);
} else if (ret < min_progress) {
// temperature is stable or increasing
min_progress = ret;
}
}
if (min_progress == 110 || abort_calibration) {
break; // we are done
}
int led_progress = min_progress * BOARD_MAX_LEDS / 100;
for (; leds_completed < led_progress; ++leds_completed) {
led_control.led_mask = 1 << leds_completed;
led_control.mode = led_control_s::MODE_ON;
publish_led_control(led_control);
}
//print progress each second
hrt_abstime now = hrt_absolute_time();
if (now > next_progress_output) {
PX4_INFO("Calibration progress: %i%%", min_progress);
next_progress_output = now + 1e6;
}
}
if (abort_calibration) {
led_control.color = led_control_s::COLOR_RED;
} else {
PX4_INFO("Sensor Measurments completed");
// save params immediately so that we can check the result and don't have to wait for param save timeout
param_control_autosave(false);
// do final calculations & parameter storage
for (int i = 0; i < num_calibrators; ++i) {
int ret = calibrators[i]->finish();
if (ret < 0) {
PX4_ERR("Failed to finish calibration process (%i)", ret);
}
}
param_notify_changes();
int ret = param_save_default();
if (ret != 0) {
PX4_ERR("Failed to save params (%i)", ret);
}
param_control_autosave(true);
led_control.color = led_control_s::COLOR_GREEN;
}
// blink the LED's according to success/failure
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_FAST;
led_control.num_blinks = 0;
publish_led_control(led_control);
for (int i = 0; i < num_calibrators; ++i) {
delete calibrators[i];
}
for (unsigned i = 0; i < num_gyro; i++) {
orb_unsubscribe(gyro_sub[i]);
}
delete temperature_calibration::instance;
temperature_calibration::instance = nullptr;
PX4_INFO("Exiting temperature calibration task");
}
int TemperatureCalibration::do_temperature_calibration(int argc, char *argv[])
{
temperature_calibration::instance->task_main();
return 0;
}
int TemperatureCalibration::start()
{
_control_task = px4_task_spawn_cmd("temperature_calib",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
5800,
(px4_main_t)&TemperatureCalibration::do_temperature_calibration,
nullptr);
if (_control_task < 0) {
delete temperature_calibration::instance;
temperature_calibration::instance = nullptr;
PX4_ERR("start failed");
return -errno;
}
return 0;
}
void TemperatureCalibration::publish_led_control(led_control_s &led_control)
{
led_control.timestamp = hrt_absolute_time();
_led_control_pub.publish(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 == nullptr) {
PX4_ERR("alloc failed");
return 1;
}
return temperature_calibration::instance->start();
}
src/modules/events/temperature_calibration/temperature_calibration.h
-40
View File
@@ -1,40 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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
/** 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);