forked from Archive/PX4-Autopilot
modules/sih: move to px4 work queue (wq:rate_ctrl)
- respect IMU_GYRO_RATEMAX for configured interval
- optionally compile at higher optimization level per board (${MAX_CUSTOM_OPT_LEVEL})
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
c01d459011
commit
bb7dd0cf00
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2019-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
|
||||
|
|
@ -34,14 +34,16 @@
|
|||
px4_add_module(
|
||||
MODULE modules__sih
|
||||
MAIN sih
|
||||
STACK_MAX 1024
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
sih.cpp
|
||||
sih.hpp
|
||||
DEPENDS
|
||||
mathlib
|
||||
drivers_accelerometer
|
||||
drivers_barometer
|
||||
drivers_gyroscope
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2019-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
|
||||
|
|
@ -47,61 +47,19 @@
|
|||
|
||||
#include <drivers/drv_pwm_output.h> // to get PWM flags
|
||||
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
|
||||
using namespace math;
|
||||
using namespace matrix;
|
||||
|
||||
|
||||
int Sih::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
|
||||
int Sih::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("sih",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX,
|
||||
1024,
|
||||
(px4_main_t)&run_trampoline,
|
||||
(char *const *)argv);
|
||||
|
||||
if (_task_id < 0) {
|
||||
_task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Sih *Sih::instantiate(int argc, char *argv[])
|
||||
{
|
||||
Sih *instance = new Sih();
|
||||
|
||||
if (instance == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
using namespace time_literals;
|
||||
|
||||
Sih::Sih() :
|
||||
ModuleParams(nullptr),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": execution")),
|
||||
_sampling_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sampling"))
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||
{
|
||||
}
|
||||
|
||||
void Sih::run()
|
||||
{
|
||||
// initialize parameters
|
||||
parameters_update_poll();
|
||||
_px4_accel.set_temperature(T1_C);
|
||||
_px4_gyro.set_temperature(T1_C);
|
||||
_px4_mag.set_temperature(T1_C);
|
||||
|
||||
parameters_updated();
|
||||
init_variables();
|
||||
init_sensors();
|
||||
|
||||
|
|
@ -109,39 +67,47 @@ void Sih::run()
|
|||
_last_run = task_start;
|
||||
_gps_time = task_start;
|
||||
_serial_time = task_start;
|
||||
}
|
||||
|
||||
px4_sem_init(&_data_semaphore, 0, 0);
|
||||
Sih::~Sih()
|
||||
{
|
||||
perf_free(_loop_perf);
|
||||
perf_free(_loop_interval_perf);
|
||||
}
|
||||
|
||||
hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, &_data_semaphore);
|
||||
bool Sih::init()
|
||||
{
|
||||
int rate = _imu_gyro_ratemax.get();
|
||||
|
||||
perf_begin(_sampling_perf);
|
||||
// default to 250 Hz (4000 us interval)
|
||||
if (rate <= 0) {
|
||||
rate = 250;
|
||||
}
|
||||
|
||||
while (!should_exit()) {
|
||||
px4_sem_wait(&_data_semaphore); // periodic real time wakeup
|
||||
// 200 - 2000 Hz
|
||||
int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
|
||||
ScheduleOnInterval(interval_us);
|
||||
|
||||
perf_end(_sampling_perf);
|
||||
perf_begin(_sampling_perf);
|
||||
return true;
|
||||
}
|
||||
|
||||
void Sih::Run()
|
||||
{
|
||||
perf_count(_loop_interval_perf);
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
inner_loop(); // main execution function
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
hrt_cancel(&_timer_call); // close the periodic timer interruption
|
||||
px4_sem_destroy(&_data_semaphore);
|
||||
}
|
||||
|
||||
// timer_callback() is used as a real time callback to post the semaphore
|
||||
void Sih::timer_callback(void *sem)
|
||||
{
|
||||
px4_sem_post((px4_sem_t *)sem);
|
||||
}
|
||||
|
||||
// this is the main execution waken up periodically by the semaphore
|
||||
void Sih::inner_loop()
|
||||
{
|
||||
_now = hrt_absolute_time();
|
||||
_dt = (_now - _last_run) * 1e-6f;
|
||||
_last_run = _now;
|
||||
|
|
@ -154,35 +120,39 @@ void Sih::inner_loop()
|
|||
|
||||
reconstruct_sensors_signals();
|
||||
|
||||
send_IMU();
|
||||
// update IMU every iteration
|
||||
_px4_accel.update(_now, _acc(0), _acc(1), _acc(2));
|
||||
_px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2));
|
||||
|
||||
if (_now - _gps_time >= 50000) { // gps published at 20Hz
|
||||
// magnetometer published at 50 Hz
|
||||
if (_now - _mag_time >= 20_ms) {
|
||||
_mag_time = _now;
|
||||
|
||||
_px4_mag.update(_now, _mag(0), _mag(1), _mag(2));
|
||||
}
|
||||
|
||||
// baro published at 20 Hz
|
||||
if (_now - _baro_time >= 50_ms) {
|
||||
_baro_time = _now;
|
||||
_px4_baro.set_temperature(_baro_temp_c);
|
||||
_px4_baro.update(_now, _baro_p_mBar);
|
||||
}
|
||||
|
||||
|
||||
// gps published at 20Hz
|
||||
if (_now - _gps_time >= 50_ms) {
|
||||
_gps_time = _now;
|
||||
send_gps();
|
||||
}
|
||||
|
||||
// send uart message every 40 ms
|
||||
if (_now - _serial_time >= 40000) {
|
||||
if (_now - _serial_time >= 40_ms) {
|
||||
_serial_time = _now;
|
||||
|
||||
publish_sih(); // publish _sih message for debug purpose
|
||||
|
||||
parameters_update_poll(); // update the parameters if needed
|
||||
}
|
||||
}
|
||||
|
||||
void Sih::parameters_update_poll()
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
parameters_updated();
|
||||
}
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
// store the parameters in a more convenient form
|
||||
|
|
@ -330,25 +300,6 @@ void Sih::reconstruct_sensors_signals()
|
|||
_gps_vel = _v_I + noiseGauss3f(0.06f, 0.077f, 0.158f);
|
||||
}
|
||||
|
||||
void Sih::send_IMU()
|
||||
{
|
||||
// gyro
|
||||
_px4_gyro.set_temperature(T1_C);
|
||||
_px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2));
|
||||
|
||||
// accel
|
||||
_px4_accel.set_temperature(T1_C);
|
||||
_px4_accel.update(_now, _acc(0), _acc(1), _acc(2));
|
||||
|
||||
// magnetometer
|
||||
_px4_mag.set_temperature(T1_C);
|
||||
_px4_mag.update(_now, _mag(0), _mag(1), _mag(2));
|
||||
|
||||
// baro
|
||||
_px4_baro.set_temperature(_baro_temp_c);
|
||||
_px4_baro.update(_now, _baro_p_mBar);
|
||||
}
|
||||
|
||||
void Sih::send_gps()
|
||||
{
|
||||
_sensor_gps.timestamp = _now;
|
||||
|
|
@ -430,9 +381,32 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
|
|||
return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz);
|
||||
}
|
||||
|
||||
int sih_main(int argc, char *argv[])
|
||||
int Sih::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
return Sih::main(argc, argv);
|
||||
Sih *instance = new Sih();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
int Sih::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int Sih::print_usage(const char *reason)
|
||||
|
|
@ -468,3 +442,8 @@ Most of the variables are declared global in the .hpp file to avoid stack overfl
|
|||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int sih_main(int argc, char *argv[])
|
||||
{
|
||||
return Sih::main(argc, argv);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2019-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
|
||||
|
|
@ -36,16 +36,17 @@
|
|||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <matrix/matrix/math.hpp> // matrix, vectors, dcm, quaterions
|
||||
#include <conversion/rotation.h> // math::radians,
|
||||
#include <ecl/geo/geo.h> // to get the physical constants
|
||||
#include <lib/ecl/geo/geo.h> // to get the physical constants
|
||||
#include <drivers/drv_hrt.h> // to get the real time
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/barometer/PX4Barometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
|
|
@ -58,46 +59,31 @@
|
|||
|
||||
using namespace time_literals;
|
||||
|
||||
extern "C" __EXPORT int sih_main(int argc, char *argv[]);
|
||||
|
||||
class Sih : public ModuleBase<Sih>, public ModuleParams
|
||||
class Sih : public ModuleBase<Sih>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
Sih();
|
||||
|
||||
virtual ~Sih() = default;
|
||||
~Sih() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static Sih *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);
|
||||
|
||||
/** @see ModuleBase::run() */
|
||||
void run() override;
|
||||
|
||||
static float generate_wgn(); // generate white Gaussian noise sample
|
||||
|
||||
// generate white Gaussian noise sample as a 3D vector with specified std
|
||||
static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
|
||||
|
||||
// timer called periodically to post the semaphore
|
||||
static void timer_callback(void *sem);
|
||||
bool init();
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
/**
|
||||
* Check for parameter changes and update them if needed.
|
||||
* @param parameter_update_sub uorb subscription to parameter_update
|
||||
* @param force for a parameter update
|
||||
*/
|
||||
void parameters_update_poll();
|
||||
void parameters_updated();
|
||||
|
||||
// simulated sensor instances
|
||||
|
|
@ -130,7 +116,6 @@ private:
|
|||
static constexpr float T1_C = 15.0f; // ground temperature in celcius
|
||||
static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin
|
||||
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
|
||||
static constexpr hrt_abstime LOOP_INTERVAL = 4000; // 4ms => 250 Hz real-time
|
||||
|
||||
void init_variables();
|
||||
void init_sensors();
|
||||
|
|
@ -138,22 +123,19 @@ private:
|
|||
void generate_force_and_torques();
|
||||
void equations_of_motion();
|
||||
void reconstruct_sensors_signals();
|
||||
void send_IMU();
|
||||
void send_gps();
|
||||
void publish_sih();
|
||||
void inner_loop();
|
||||
|
||||
perf_counter_t _loop_perf;
|
||||
perf_counter_t _sampling_perf;
|
||||
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
|
||||
|
||||
px4_sem_t _data_semaphore;
|
||||
|
||||
hrt_call _timer_call;
|
||||
hrt_abstime _last_run;
|
||||
hrt_abstime _gps_time;
|
||||
hrt_abstime _serial_time;
|
||||
hrt_abstime _now;
|
||||
float _dt; // sampling time [s]
|
||||
hrt_abstime _last_run{0};
|
||||
hrt_abstime _baro_time{0};
|
||||
hrt_abstime _gps_time{0};
|
||||
hrt_abstime _mag_time{0};
|
||||
hrt_abstime _serial_time{0};
|
||||
hrt_abstime _now{0};
|
||||
float _dt{0}; // sampling time [s]
|
||||
bool _grounded{true};// whether the vehicle is on the ground
|
||||
|
||||
matrix::Vector3f _T_B; // thrust force in body frame [N]
|
||||
|
|
@ -194,6 +176,8 @@ private:
|
|||
|
||||
// parameters defined in sih_params.c
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _imu_gyro_ratemax,
|
||||
|
||||
(ParamFloat<px4::params::SIH_MASS>) _sih_mass,
|
||||
(ParamFloat<px4::params::SIH_IXX>) _sih_ixx,
|
||||
(ParamFloat<px4::params::SIH_IYY>) _sih_iyy,
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2019-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
|
||||
|
|
|
|||
Loading…
Reference in New Issue