fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 06:30:35 +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/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);