Compare commits

...

15 Commits

Author SHA1 Message Date
Julien Lecoeur 1bb0bf95d3 Fixup vehicle_model 2019-11-25 19:19:09 +01:00
Julien Lecoeur f8c2b0d661 [WIP] test in SITL 2019-11-24 19:30:20 +01:00
Julien Lecoeur 96891df222 logger: log vehicle_model for system identification 2019-11-24 19:30:05 +01:00
Julien Lecoeur 96c9f56e1b Add module vehicle_model_estimator
This module estimates the vehicle mass and inertia matrix.

It uses a least mean square algorithm between
delayed, low pass filtered, and time differentiated setpoints
(thrust and torque, respectively),
and low pass filtered and time differentiated accelerations
(linear and angular, respectively).
2019-11-24 19:29:39 +01:00
Julien Lecoeur fa842c7caf fmu-v2: save flash 2019-11-24 12:06:40 +01:00
Julien Lecoeur 81561fd843 omnibus_f4sd_default: save flash 2019-11-24 12:06:40 +01:00
Julien Lecoeur 3a1e8f8172 intel_aerofc-v1_default: save flash 2019-11-24 12:06:40 +01:00
Julien Lecoeur 0bbfc36f11 fw_att_control: publich torque&thrust sp for VTOL during fixed wing mode 2019-11-24 12:06:40 +01:00
Julien Lecoeur 0fede1ff50 mc_rate_control: publish torque&thrust sp for VTOLs during hover and transition 2019-11-24 12:06:40 +01:00
Julien Lecoeur 2b98add9ec sitl iris: set inertia and update gains 2019-11-24 12:06:40 +01:00
Julien Lecoeur 62de8a2c5b mc_rate_control: add inertia matrix 2019-11-24 11:30:26 +01:00
Julien Lecoeur 4e1ce3393e logger: log angular acceleration, torque and thrust setpoints for system identification 2019-11-24 11:29:55 +01:00
Julien Lecoeur 63d5155587 uORB: Add angular accel, torque & thrust setpoints 2019-11-24 11:29:55 +01:00
Julien Lecoeur 3e4137ffe8 Add vehicle_angular_acceleration topic and sensor 2019-11-24 11:29:55 +01:00
Julien Lecoeur 7cb029288a LPF: add sample frequency getter 2019-11-24 11:29:55 +01:00
36 changed files with 1592 additions and 50 deletions

View File

@ -9,5 +9,19 @@
sh /etc/init.d/rc.mc_defaults
if [ $AUTOCNF = yes ]
then
param set MC_ROLLRATE_P 18.0
param set MC_ROLLRATE_D 0.36
param set MC_PITCHRATE_P 18.0
param set MC_PITCHRATE_D 0.36
param set MC_YAWRATE_P 7.0
param set MC_YAWRATE_D 0.0
param set VM_INERTIA_XX 0.0083
param set VM_INERTIA_YY 0.0083
param set VM_INERTIA_ZZ 0.0286
fi
set MIXER quad_w
vehicle_model_estimator start

View File

@ -44,7 +44,7 @@ px4_add_board(
land_detector
landing_target_estimator
load_mon
local_position_estimator
#local_position_estimator
logger
mavlink
mc_att_control

View File

@ -88,7 +88,7 @@ px4_add_board(
shutdown
#tests # tests and test runner
top
topic_listener
#topic_listener
tune_control
usb_connected
ver

View File

@ -48,12 +48,12 @@ px4_add_board(
magnetometer/hmc5883
#mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#optical_flow/px4flow
#osd
#pca9685
#protocol_splitter
#pwm_input
pwm_out_sim
#pwm_out_sim
px4fmu
px4io
#roboclaw

View File

@ -43,6 +43,7 @@ px4_add_board(
sensors
#sih
simulator
vehicle_model_estimator
vmount
vtol_att_control
SYSTEMCMDS

View File

@ -130,6 +130,8 @@ set(msg_files
ulog_stream_ack.msg
vehicle_acceleration.msg
vehicle_air_data.msg
vehicle_angular_acceleration.msg
vehicle_angular_acceleration_setpoint.msg
vehicle_angular_velocity.msg
vehicle_attitude.msg
vehicle_attitude_setpoint.msg
@ -143,11 +145,14 @@ set(msg_files
vehicle_local_position.msg
vehicle_local_position_setpoint.msg
vehicle_magnetometer.msg
vehicle_model.msg
vehicle_odometry.msg
vehicle_rates_setpoint.msg
vehicle_roi.msg
vehicle_status.msg
vehicle_status_flags.msg
vehicle_thrust_setpoint.msg
vehicle_torque_setpoint.msg
vehicle_trajectory_waypoint.msg
vtol_vehicle_status.msg
wheel_encoders.msg

View File

@ -265,6 +265,14 @@ rtps:
id: 116
- msg: cellular_status
id: 117
- msg: vehicle_angular_acceleration
id: 118
- msg: vehicle_angular_acceleration_setpoint
id: 119
- msg: vehicle_torque_setpoint
id: 120
- msg: vehicle_thrust_setpoint
id: 121
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

View File

@ -0,0 +1,7 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2,
# computed by differentiating vehicle_angular_velocity and applying a low pass filter
# can be used to verify that the vehicle correctly tracks angular acceleration setpoints

View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2

6
msg/vehicle_model.msg Normal file
View File

@ -0,0 +1,6 @@
# Vehicle model, mass and inertia matrix
uint64 timestamp # time since system start (microseconds)
float32 mass # vehicle mass
float32[9] inertia # vehicle inertia matrix

View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # thrust setpoint along X, Y, Z body axis (in N)

View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # torque setpoint about X, Y, Z body axis (in N.m)

View File

@ -43,6 +43,7 @@ namespace math
void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
_sample_freq = sample_freq;
// reset delay elements on filter change
_delay_element_1.zero();

View File

@ -74,12 +74,16 @@ public:
// Return the cutoff frequency
float get_cutoff_freq() const { return _cutoff_freq; }
// Return the sample frequency
float get_sample_freq() const { return _sample_freq; }
// Reset the filter state to this value
matrix::Vector3f reset(const matrix::Vector3f &sample);
private:
float _cutoff_freq{0.0f};
float _sample_freq{0.0f};
float _a1{0.0f};
float _a2{0.0f};

View File

@ -655,6 +655,28 @@ void FixedwingAttitudeControl::Run()
}
_actuators_2_pub.publish(_actuators_airframe);
/* publish thrust and torque setpoint */
if (!_vehicle_status.is_vtol
|| (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING))
) {
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = _att.timestamp;
v_torque_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[0])) ? _actuators.control[0] : 0.0f;
v_torque_sp.xyz[1] = (PX4_ISFINITE(_actuators.control[1])) ? _actuators.control[1] : 0.0f;
v_torque_sp.xyz[2] = (PX4_ISFINITE(_actuators.control[2])) ? _actuators.control[2] : 0.0f;
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = _att.timestamp;
v_thrust_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[3])) ? _actuators.control[3] : 0.0f;
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
}
}

View File

@ -66,6 +66,8 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using matrix::Eulerf;
using matrix::Quatf;
@ -115,6 +117,8 @@ private:
uORB::Publication<actuator_controls_s> _actuators_2_pub{ORB_ID(actuator_controls_2)}; /**< actuator control group 1 setpoint (Airframe) */
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< rate controller status publication */
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle_thrust_setpoint publication */
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< vehicle_torque_setpoint publication */
orb_id_t _attitude_setpoint_id{nullptr};
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */

View File

@ -653,6 +653,10 @@ void Logger::add_system_identification_topics()
add_topic("actuator_controls_0");
add_topic("actuator_controls_1");
add_topic("sensor_combined");
add_topic("vehicle_angular_acceleration");
add_topic("vehicle_angular_acceleration_setpoint");
add_topic("vehicle_torque_setpoint");
add_topic("vehicle_model");
}
int Logger::add_topics_from_file(const char *fname)

View File

@ -94,6 +94,14 @@ MulticopterRateControl::parameters_updated()
_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
radians(_param_mc_acro_y_max.get()));
// inertia matrix
const float inertia[3][3] = {
{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
};
_rate_control.setInertiaMatrix(matrix::Matrix3f(inertia));
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
}
@ -164,6 +172,7 @@ MulticopterRateControl::Run()
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
const hrt_abstime now = hrt_absolute_time();
_timestamp_sample = angular_velocity.timestamp_sample;
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@ -271,7 +280,7 @@ MulticopterRateControl::Run()
}
// run the rate controller
if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
if (_v_control_mode.flag_control_rates_enabled) {
// reset integral if disarmed
if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@ -291,7 +300,7 @@ MulticopterRateControl::Run()
}
// run rate controller
const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
_rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
@ -299,41 +308,26 @@ MulticopterRateControl::Run()
rate_ctrl_status.timestamp = hrt_absolute_time();
_controller_status_pub.publish(rate_ctrl_status);
// publish actuator controls
actuator_controls_s actuators{};
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// scale effort by battery status if enabled
if (_param_mc_bat_scale_en.get()) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status;
if (_battery_status_sub.copy(&battery_status)) {
_battery_status_scale = battery_status.scale;
}
}
if (_battery_status_scale > 0.0f) {
for (int i = 0; i < 4; i++) {
actuators.control[i] *= _battery_status_scale;
}
}
// publish controller output
if (!_vehicle_status.is_vtol
|| (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING))
|| (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode)) {
publish_actuator_controls();
publish_angular_acceleration_setpoint();
publish_torque_setpoint();
publish_thrust_setpoint();
}
actuators.timestamp = hrt_absolute_time();
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
} else if (_v_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
// publish actuator controls
actuator_controls_s actuators{};
actuators.timestamp = hrt_absolute_time();
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
// reset controller
_rate_control.reset();
// publish controller output
publish_actuator_controls();
publish_angular_acceleration_setpoint();
publish_torque_setpoint();
publish_thrust_setpoint();
}
}
}
@ -341,6 +335,91 @@ MulticopterRateControl::Run()
perf_end(_loop_perf);
}
void
MulticopterRateControl::publish_actuator_controls()
{
Vector3f act_control = _rate_control.getTorqueSetpoint();
actuator_controls_s actuators{};
actuators.timestamp = hrt_absolute_time();
actuators.timestamp_sample = _timestamp_sample;
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(act_control(0)) ? act_control(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(act_control(1)) ? act_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(act_control(2)) ? act_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
// scale effort by battery status if enabled
if (_param_mc_bat_scale_en.get()) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status;
if (_battery_status_sub.copy(&battery_status)) {
_battery_status_scale = battery_status.scale;
}
}
if (_battery_status_scale > 0.0f) {
for (int i = 0; i < 4; i++) {
actuators.control[i] *= _battery_status_scale;
}
}
}
if (!_actuators_0_circuit_breaker_enabled) {
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
}
}
void
MulticopterRateControl::publish_angular_acceleration_setpoint()
{
Vector3f angular_accel_sp = _rate_control.getAngularAccelerationSetpoint();
vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
v_angular_accel_sp.timestamp = hrt_absolute_time();
v_angular_accel_sp.timestamp_sample = _timestamp_sample;
v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
if (!_vehicle_status.is_vtol) {
_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
}
}
void
MulticopterRateControl::publish_torque_setpoint()
{
Vector3f torque_sp = _rate_control.getTorqueSetpoint();
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = _timestamp_sample;
v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
if (!_vehicle_status.is_vtol) {
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
}
void
MulticopterRateControl::publish_thrust_setpoint()
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = _timestamp_sample;
v_thrust_sp.xyz[0] = 0.0f;
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = (PX4_ISFINITE(-_thrust_sp)) ? (-_thrust_sp) : 0.0f;
if (!_vehicle_status.is_vtol) {
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
}
int MulticopterRateControl::task_spawn(int argc, char *argv[])
{
MulticopterRateControl *instance = new MulticopterRateControl();

View File

@ -54,11 +54,15 @@
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
{
@ -98,6 +102,11 @@ private:
*/
float get_landing_gear_state();
void publish_angular_acceleration_setpoint();
void publish_torque_setpoint();
void publish_thrust_setpoint();
void publish_actuator_controls();
RateControl _rate_control; ///< class for rate control calculations
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
@ -115,6 +124,9 @@ private:
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< torque setpoint publication */
orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
@ -143,6 +155,7 @@ private:
hrt_abstime _task_start{hrt_absolute_time()};
hrt_abstime _last_run{0};
hrt_abstime _timestamp_sample{0};
float _dt_accumulator{0.0f};
int _loop_counter{0};
@ -184,7 +197,14 @@ private:
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl,
(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx, /**< vehicle inertia */
(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz
)
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */

View File

@ -66,7 +66,7 @@ void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &
_mixer_saturation_negative[2] = status.flags.yaw_neg;
}
Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed)
void RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed)
{
// angular rates error
Vector3f rate_error = rate_sp - rate;
@ -79,9 +79,28 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
rate_d = (rate_filtered - _rate_prev_filtered) / dt;
}
// PID control with feed forward
const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
// P + D control
_angular_accel_sp = _gain_p.emult(rate_error) - _gain_d.emult(rate_d);
// I + FF control
Vector3f torque_feedforward = _rate_int + _gain_ff.emult(rate_sp);
// compute torque setpoint
// Note: this may go into a separate module for general usage with FW and VTOLs
// if so, TODO:
// - [x] publish accel sp
// - [ ] publish torque ff sp
// - [ ] add dynamic model module
// - [ ] move params for inertia to that module
// - [ ] poll vehicle_angular_velocity & vehicle_angular_acceleration_setpoint & vehicle_torque_feedforward_setpoint => compute and publish vehicle_torque_setpoint
// - [ ] (eventually) add param for mass, poll vehicle_linear_acceleration_setpoint + vehicle_attitude => compute and publish vehicle_thrust_setpoint
_torque_sp = (
_inertia * _angular_accel_sp
+ torque_feedforward
+ rate.cross(_inertia * rate)
);
// save states
_rate_prev = rate;
_rate_prev_filtered = rate_filtered;
@ -89,8 +108,6 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
if (!landed) {
updateIntegral(rate_error, dt);
}
return torque;
}
void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
@ -125,6 +142,15 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
}
}
void RateControl::reset()
{
_rate_int.zero();
_rate_prev.zero();
_rate_prev_filtered.zero();
_torque_sp.zero();
_angular_accel_sp.zero();
}
void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
{
rate_ctrl_status.rollspeed_integ = _rate_int(0);

View File

@ -80,6 +80,13 @@ public:
*/
void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
/**
* Set inertia matrix
* @see _inertia
* @param inertia inertia matrix
*/
void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
/**
* Set saturation status
* @param status message from mixer reporting about saturation
@ -91,10 +98,20 @@ public:
* @param rate estimation of the current vehicle angular rate
* @param rate_sp desired vehicle angular rate setpoint
* @param dt desired vehicle angular rate setpoint
* @return [-1,1] normalized torque vector to apply to the vehicle
*/
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt,
const bool landed);
void update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed);
/**
* Get the desired angular acceleration
* @see _angular_accel_sp
*/
const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
/**
* Get the torque vector to apply to the vehicle
* @see _torque_sp
*/
const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
/**
* Set the integral term to 0 to prevent windup
@ -102,6 +119,11 @@ public:
*/
void resetIntegral() { _rate_int.zero(); }
/**
* ReSet the controller state
*/
void reset();
/**
* Get status message of controller for logging/debugging
* @param rate_ctrl_status status message to fill with internal states
@ -117,6 +139,7 @@ private:
matrix::Vector3f _gain_d; ///< rate control derivative gain
matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
// States
matrix::Vector3f _rate_prev; ///< angular rates of previous update
@ -125,4 +148,8 @@ private:
math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw)
bool _mixer_saturation_positive[3] {};
bool _mixer_saturation_negative[3] {};
// Output
matrix::Vector3f _angular_accel_sp; //< Angular acceleration setpoint computed using P and D gains
matrix::Vector3f _torque_sp; //< Torque setpoint to apply to the vehicle
};

View File

@ -39,6 +39,7 @@ using namespace matrix;
TEST(RateControlTest, AllZeroCase)
{
RateControl rate_control;
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false);
rate_control.update(Vector3f(), Vector3f(), 0.f, false);
Vector3f torque = rate_control.getTorqueSetpoint();
EXPECT_EQ(torque, Vector3f());
}

View File

@ -412,4 +412,3 @@ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
* @group Multicopter Rate Control
*/
PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);

View File

@ -33,6 +33,7 @@
add_subdirectory(vehicle_acceleration)
add_subdirectory(vehicle_angular_velocity)
add_subdirectory(vehicle_angular_acceleration)
px4_add_module(
MODULE modules__sensors
@ -54,4 +55,5 @@ px4_add_module(
mathlib
vehicle_acceleration
vehicle_angular_velocity
vehicle_angular_acceleration
)

View File

@ -155,6 +155,8 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
parameter_handles.imu_dgyro_en = param_find("IMU_DGYRO_EN");
// These are parameters for which QGroundControl always expects to be returned in a list request.
// We do a param_find here to force them into the list.
(void)param_find("RC_CHAN_CNT");
@ -385,6 +387,8 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);
param_get(parameter_handles.imu_dgyro_en, &parameters.imu_dgyro_en);
return ret;
}

View File

@ -143,6 +143,8 @@ struct Parameters {
int32_t air_cmodel;
float air_tube_length;
float air_tube_diameter_mm;
int32_t imu_dgyro_en;
};
struct ParameterHandles {
@ -222,6 +224,7 @@ struct ParameterHandles {
param_t air_tube_length;
param_t air_tube_diameter_mm;
param_t imu_dgyro_en;
};
/**

View File

@ -227,6 +227,31 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
/**
* Enable computation of angular acceleration by gyro differentiation.
*
* @reboot_required true
*
* @boolean
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_DGYRO_EN, 1);
/**
* Cutoff frequency for angular acceleration
*
* The cutoff frequency for the 2nd order butterworth filter used on
* the time derivative of the measured angular velocity.
* Set to 0 to disable the filter.
*
* @min 0
* @max 1000
* @unit Hz
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);
/**
* Gyro control data maximum publication rate
*

View File

@ -92,6 +92,7 @@
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_angular_acceleration/VehicleAngularAcceleration.hpp"
using namespace DriverFramework;
using namespace sensors;
@ -170,8 +171,9 @@ private:
VotedSensorsUpdate _voted_sensors_update;
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAcceleration _vehicle_acceleration;
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAngularAcceleration _vehicle_angular_acceleration;
/**
@ -215,18 +217,24 @@ Sensors::Sensors(bool hil_enabled) :
_voted_sensors_update(_parameters, hil_enabled)
{
initialize_parameter_handles(_parameter_handles);
parameters_update();
_airspeed_validator.set_timeout(300000);
_airspeed_validator.set_equal_value_threshold(100);
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
if (_parameters.imu_dgyro_en) {
_vehicle_angular_acceleration.Start();
}
}
Sensors::~Sensors()
{
_vehicle_acceleration.Stop();
_vehicle_angular_velocity.Stop();
_vehicle_angular_acceleration.Stop();
}
int
@ -573,6 +581,7 @@ int Sensors::print_status()
_vehicle_acceleration.PrintStatus();
_vehicle_angular_velocity.PrintStatus();
_vehicle_angular_acceleration.PrintStatus();
return 0;
}

View File

@ -0,0 +1,37 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(vehicle_angular_acceleration
VehicleAngularAcceleration.cpp
)
target_link_libraries(vehicle_angular_acceleration PRIVATE px4_work_queue)

View File

@ -0,0 +1,156 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "VehicleAngularAcceleration.hpp"
#include <px4_log.h>
using namespace matrix;
using namespace time_literals;
VehicleAngularAcceleration::VehicleAngularAcceleration() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_acceleration: cycle time")),
_sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_acceleration: sensor latency"))
{
}
VehicleAngularAcceleration::~VehicleAngularAcceleration()
{
Stop();
perf_free(_cycle_perf);
perf_free(_sensor_latency_perf);
}
bool
VehicleAngularAcceleration::Start()
{
_angular_velocity_prev.zero();
// force initial updates
ParametersUpdate(true);
// Register callbacks
_vehicle_angular_velocity_sub.registerCallback();
return true;
}
void
VehicleAngularAcceleration::Stop()
{
Deinit();
// clear all registered callbacks
_vehicle_angular_velocity_sub.unregisterCallback();
}
void
VehicleAngularAcceleration::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
updateParams();
// Low pass filter
if (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
_lp_filter.set_cutoff_frequency(_loop_update_rate_hz, _param_imu_dgyro_cutoff.get());
_lp_filter.reset(_angular_velocity_prev);
}
}
}
void
VehicleAngularAcceleration::Run()
{
perf_begin(_cycle_perf);
vehicle_angular_velocity_s v_angular_velocity;
if (_vehicle_angular_velocity_sub.update(&v_angular_velocity)) {
perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&v_angular_velocity.timestamp));
ParametersUpdate();
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt = math::constrain(((v_angular_velocity.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f,
0.02f);
_timestamp_sample_prev = v_angular_velocity.timestamp_sample;
// Differentiate angular velocity
Vector3f angular_velocity(v_angular_velocity.xyz);
Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
_angular_velocity_prev = angular_velocity;
// Apply low pass filter
Vector3f angular_acceleration(_lp_filter.apply(angular_acceleration_raw));
// Publish
vehicle_angular_acceleration_s v_angular_acceleration;
v_angular_acceleration.timestamp = hrt_absolute_time();
v_angular_acceleration.timestamp_sample = v_angular_velocity.timestamp_sample;
angular_acceleration.copyTo(v_angular_acceleration.xyz);
_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
// Calculate loop rate
_dt_accumulator += dt;
++_loop_counter;
if (_dt_accumulator > 1.f) {
const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
_dt_accumulator = 0;
_loop_counter = 0;
if (fabsf(_lp_filter.get_sample_freq() - _loop_update_rate_hz) > 1.0f) {
_lp_filter.set_cutoff_frequency(_loop_update_rate_hz, _param_imu_dgyro_cutoff.get());
}
}
}
perf_end(_cycle_perf);
}
void
VehicleAngularAcceleration::PrintStatus()
{
perf_print_counter(_cycle_perf);
perf_print_counter(_sensor_latency_perf);
}

View File

@ -0,0 +1,91 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/mathlib/math/Limits.hpp>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
class VehicleAngularAcceleration : public ModuleParams, public px4::WorkItem
{
public:
VehicleAngularAcceleration();
virtual ~VehicleAngularAcceleration();
void Run() override;
bool Start();
void Stop();
void PrintStatus();
private:
void ParametersUpdate(bool force = false);
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
)
uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; /**< angular velocity subscription */
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
math::LowPassFilter2pVector3f _lp_filter{initial_update_rate_hz, 10.f}; /**< low-pass filter for angular acceleration */
static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */
matrix::Vector3f _angular_velocity_prev;
hrt_abstime _timestamp_sample_prev{0};
float _dt_accumulator{0.0f};
int _loop_counter{0};
perf_counter_t _cycle_perf;
perf_counter_t _sensor_latency_perf;
};

View File

@ -0,0 +1,45 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__vehicle_model_estimator
MAIN vehicle_model_estimator
COMPILE_FLAGS
SRCS
VehicleModelEstimator.cpp
VehicleModelEstimator.hpp
DEPENDS
mathlib
px4_work_queue
ecl_EKF
)

View File

@ -0,0 +1,519 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file VehicleModelEstimator.cpp
*
* Vehicle model estimator.
*
* To estimate the vehicle inertia, the estimator low-pass filters and
* computes the time derivative of the vehicle angular acceleration
* and of the vehicle torque setpoint, then applies a least-mean-square.
* Similarly, to estimate the vehicle mass, the estimator low-pass
* filters and computes the time derivative of the vehicle acceleration
* and of the vehicle thrust setpoint, then applies a least-mean-square.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "VehicleModelEstimator.hpp"
#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
using namespace matrix;
using namespace time_literals;
VehicleModelEstimator::VehicleModelEstimator() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
parameters_updated();
_I_est_inv = inv(_I);
_mass_est_inv = 1.0f / _mass;
}
VehicleModelEstimator::~VehicleModelEstimator()
{
perf_free(_loop_perf);
}
bool
VehicleModelEstimator::init()
{
if (!_vehicle_angular_acceleration_sub.registerCallback()) {
PX4_ERR("vehicle_angular_acceleration callback registration failed!");
return false;
}
if (!_vehicle_acceleration_sub.registerCallback()) {
PX4_ERR("vehicle_acceleration callback registration failed!");
return false;
}
return true;
}
void
VehicleModelEstimator::parameters_updated()
{
// vehicle inertia matrix
const float inertia[3][3] = {
{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
};
_I = matrix::Matrix3f(inertia);
// vehicle mass
_mass = _param_vm_mass.get();
// low pass filters
if (fabsf(_lpf_ang_accel.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
_lpf_ang_accel.set_cutoff_frequency(_loop_update_rate_hz_ang_accel, _param_vm_est_cutoff.get());
_lpf_ang_accel.reset(_ang_accel_filtered);
}
if (fabsf(_lpf_torque_sp.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
_lpf_torque_sp.set_cutoff_frequency(_loop_update_rate_hz_torque_sp, _param_vm_est_cutoff.get());
_lpf_torque_sp.reset(_torque_sp_filtered);
}
if (fabsf(_lpf_lin_accel.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
_lpf_lin_accel.set_cutoff_frequency(_loop_update_rate_hz_lin_accel, _param_vm_est_cutoff.get());
_lpf_lin_accel.reset(_lin_accel_filtered);
}
if (fabsf(_lpf_thrust_sp.get_cutoff_freq() - _param_vm_est_cutoff.get()) > 0.01f) {
_lpf_thrust_sp.set_cutoff_frequency(_loop_update_rate_hz_thrust_sp, _param_vm_est_cutoff.get());
_lpf_thrust_sp.reset(_thrust_sp_filtered);
}
}
void
VehicleModelEstimator::Run()
{
if (should_exit()) {
_vehicle_angular_acceleration_sub.unregisterCallback();
_vehicle_acceleration_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
parameters_updated();
}
// check for updates in other topics
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
_maybe_landed = vehicle_land_detected.maybe_landed;
}
}
if (_vehicle_control_mode_sub.updated()) {
vehicle_control_mode_s vehicle_control_mode;
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
_armed = vehicle_control_mode.flag_armed;
}
}
const hrt_abstime now = hrt_absolute_time();
/* run inertia estimator on angular acceleration and torque setpoint changes */
bool inertia_update = false;
vehicle_angular_acceleration_s angular_acceleration;
vehicle_torque_setpoint_s torque_setpoint;
if (_vehicle_angular_acceleration_sub.update(&angular_acceleration)) {
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt_ang_accel = math::constrain(((angular_acceleration.timestamp_sample - _timestamp_ang_accel) / 1e6f), 0.0002f, 0.02f);
_timestamp_ang_accel = angular_acceleration.timestamp_sample;
// Calculate loop update rate
// (while disarmed or at least a few times beacause updating the filter is expensive)
if (!_armed || (now - _task_start) < 3300000) {
_dt_accumulator_ang_accel += dt_ang_accel;
++_loop_counter_ang_accel;
if (_dt_accumulator_ang_accel > 1.0f) {
const float loop_update_rate = (float)_loop_counter_ang_accel / _dt_accumulator_ang_accel;
_loop_update_rate_hz_ang_accel = _loop_update_rate_hz_ang_accel * 0.5f + loop_update_rate * 0.5f;
_dt_accumulator_ang_accel = 0.0f;
_loop_counter_ang_accel = 0;
if (fabsf(_lpf_ang_accel.get_sample_freq() - _loop_update_rate_hz_ang_accel) > 1.0f) {
_lpf_ang_accel.set_cutoff_frequency(_loop_update_rate_hz_ang_accel, _param_vm_est_cutoff.get());
}
}
}
// Apply low pass filter
Vector3f ang_accel_filtered_new(_lpf_ang_accel.apply(Vector3f(angular_acceleration.xyz)));
// Time derivative
_ang_accel_filtered_diff = (ang_accel_filtered_new - _ang_accel_filtered) / dt_ang_accel;
_ang_accel_filtered = ang_accel_filtered_new;
// Indicate that new data is available to estimate the vehicle inertia
inertia_update = true;
}
if (_vehicle_torque_setpoint_sub.update(&torque_setpoint)) {
// Buffer incoming data
_torque_sp_buffer.push(torque_setpoint);
torque_setpoint = _torque_sp_buffer.get_oldest();
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt_torque_sp = math::constrain(((torque_setpoint.timestamp_sample - _timestamp_torque_sp) / 1e6f), 0.0002f, 0.02f);
_timestamp_torque_sp = torque_setpoint.timestamp_sample;
// Calculate loop update rate
// (while disarmed or at least a few times beacause updating the filter is expensive)
if (!_armed || (now - _task_start) < 3300000) {
_dt_accumulator_torque_sp += dt_torque_sp;
++_loop_counter_torque_sp;
if (_dt_accumulator_torque_sp > 1.0f) {
const float loop_update_rate = (float)_loop_counter_torque_sp / _dt_accumulator_torque_sp;
_loop_update_rate_hz_torque_sp = _loop_update_rate_hz_torque_sp * 0.5f + loop_update_rate * 0.5f;
_dt_accumulator_torque_sp = 0.0f;
_loop_counter_torque_sp = 0;
if (fabsf(_lpf_torque_sp.get_sample_freq() - _loop_update_rate_hz_torque_sp) > 1.0f) {
_lpf_torque_sp.set_cutoff_frequency(_loop_update_rate_hz_torque_sp, _param_vm_est_cutoff.get());
}
}
}
// Compute required buffer size
if (!_armed || (now - _task_start) < 3300000 || _torque_sp_buffer.get_length() < 2) {
int _torque_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_torque_sp) + 1;
if (_torque_sp_buffer.get_length() != _torque_sp_buffer_length) {
if(!_torque_sp_buffer.allocate(_torque_sp_buffer_length)) {
PX4_ERR("Torque setpoint buffer allocation failed!");
}
}
}
// Apply low pass filter
Vector3f torque_sp_filtered_new(_lpf_torque_sp.apply(Vector3f(torque_setpoint.xyz)));
// Time derivative
_torque_sp_filtered_diff = (torque_sp_filtered_new - _torque_sp_filtered) / dt_torque_sp;
_torque_sp_filtered = torque_sp_filtered_new;
// Indicate that new data is available to estimate the vehicle inertia
inertia_update = true;
}
/* run mass estimator on linear acceleration and thrust setpoint changes */
bool mass_update = false;
vehicle_acceleration_s acceleration;
vehicle_thrust_setpoint_s thrust_setpoint;
if (_vehicle_acceleration_sub.update(&acceleration)) {
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt_lin_accel = math::constrain(((acceleration.timestamp_sample - _timestamp_lin_accel) / 1e6f), 0.0002f, 0.02f);
_timestamp_lin_accel = acceleration.timestamp_sample;
// Calculate loop update rate
// (while disarmed or at least a few times beacause updating the filter is expensive)
if (!_armed || (now - _task_start) < 3300000) {
_dt_accumulator_lin_accel += dt_lin_accel;
++_loop_counter_lin_accel;
if (_dt_accumulator_lin_accel > 1.0f) {
const float loop_update_rate = (float)_loop_counter_lin_accel / _dt_accumulator_lin_accel;
_loop_update_rate_hz_lin_accel = _loop_update_rate_hz_lin_accel * 0.5f + loop_update_rate * 0.5f;
_dt_accumulator_lin_accel = 0.0f;
_loop_counter_lin_accel = 0;
if (fabsf(_lpf_lin_accel.get_sample_freq() - _loop_update_rate_hz_lin_accel) > 1.0f) {
_lpf_lin_accel.set_cutoff_frequency(_loop_update_rate_hz_lin_accel, _param_vm_est_cutoff.get());
}
}
}
// Apply low pass filter
Vector3f lin_accel_filtered_new(_lpf_lin_accel.apply(Vector3f(acceleration.xyz)));
// Time derivative
_lin_accel_filtered_diff = (lin_accel_filtered_new - _lin_accel_filtered) / dt_lin_accel;
_lin_accel_filtered = lin_accel_filtered_new;
// Indicate that new data is available to estimate the vehicle mass
mass_update = true;
}
if (_vehicle_thrust_setpoint_sub.update(&thrust_setpoint)) {
// Buffer incoming data
_thrust_sp_buffer.push(thrust_setpoint);
thrust_setpoint = _thrust_sp_buffer.get_oldest();
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const float dt_thrust_sp = math::constrain(((thrust_setpoint.timestamp_sample - _timestamp_thrust_sp) / 1e6f), 0.0002f, 0.02f);
_timestamp_thrust_sp = thrust_setpoint.timestamp_sample;
// Calculate loop update rate
// (while disarmed or at least a few times beacause updating the filter is expensive)
if (!_armed || (now - _task_start) < 3300000) {
_dt_accumulator_thrust_sp += dt_thrust_sp;
++_loop_counter_thrust_sp;
if (_dt_accumulator_thrust_sp > 1.0f) {
const float loop_update_rate = (float)_loop_counter_thrust_sp / _dt_accumulator_thrust_sp;
_loop_update_rate_hz_thrust_sp = _loop_update_rate_hz_thrust_sp * 0.5f + loop_update_rate * 0.5f;
_dt_accumulator_thrust_sp = 0.0f;
_loop_counter_thrust_sp = 0;
if (fabsf(_lpf_thrust_sp.get_sample_freq() - _loop_update_rate_hz_thrust_sp) > 1.0f) {
_lpf_thrust_sp.set_cutoff_frequency(_loop_update_rate_hz_thrust_sp, _param_vm_est_cutoff.get());
}
}
}
// Compute required buffer size
if (!_armed || (now - _task_start) < 3300000 || _thrust_sp_buffer.get_length() < 2) {
int _thrust_sp_buffer_length = (_param_vm_est_delay.get() * _loop_update_rate_hz_thrust_sp) + 1;
if (_thrust_sp_buffer.get_length() != _thrust_sp_buffer_length) {
if(!_thrust_sp_buffer.allocate(_thrust_sp_buffer_length)) {
PX4_ERR("Thrust setpoint buffer allocation failed!");
}
}
}
// Apply low pass filter
Vector3f thrust_sp_filtered_new(_lpf_thrust_sp.apply(Vector3f(thrust_setpoint.xyz)));
// Time derivative
_thrust_sp_filtered_diff = (thrust_sp_filtered_new - _thrust_sp_filtered) / dt_thrust_sp;
_thrust_sp_filtered = thrust_sp_filtered_new;
// Indicate that new data is available to estimate the vehicle mass
mass_update = true;
}
// Update inertia estimate only if we have new data and we are flying
if (inertia_update && _armed && !_landed && !_maybe_landed) {
// Predict angular jerk from time derivative of torque setpoint and estimated inertia
Matrix<float, 3, 1> jerk_pred = _I_est_inv * _torque_sp_filtered_diff;
// Compute prediction error
matrix::Matrix<float, 3, 1> error = _ang_accel_filtered_diff - jerk_pred;
// Update estimate
_I_est_inv += error * _torque_sp_filtered_diff.T() * _param_vm_est_gain.get();
// Sanity check
bool reset_needed = false;
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (not PX4_ISFINITE(_I_est_inv(i, j))) {
reset_needed = true;
}
}
}
if (reset_needed) {
_I_est_inv = inv(_I);
}
// Enforce positive values on diagonal
for (size_t i = 0; i < 3; i++) {
if (_I_est_inv(i, i) < FLT_EPSILON) {
_I_est_inv(i, i) = FLT_EPSILON;
}
}
// Enforce symmetry
_I_est_inv(1, 0) = 0.5f * (_I_est_inv(0, 1) + _I_est_inv(1, 0));
_I_est_inv(0, 1) = _I_est_inv(1, 0);
_I_est_inv(2, 0) = 0.5f * (_I_est_inv(0, 2) + _I_est_inv(2, 0));
_I_est_inv(0, 2) = _I_est_inv(2, 0);
_I_est_inv(2, 1) = 0.5f * (_I_est_inv(1, 2) + _I_est_inv(2, 1));
_I_est_inv(1, 2) = _I_est_inv(2, 1);
}
// Update mass estimate only if we have new data and we are flying
if (mass_update && _armed && !_landed && !_maybe_landed) {
// Predict jerk from time derivative of thrust setpoint and estimated mass
Vector3f jerk_pred = _mass_est_inv * _thrust_sp_filtered_diff;
// Compute prediction error
Vector3f error = _lin_accel_filtered_diff - jerk_pred;
// Update estimate
_mass_est_inv += error.dot(_torque_sp_filtered_diff) * _param_vm_est_gain.get();
// Sanity check
if (not PX4_ISFINITE(_mass_est_inv)) {
_mass_est_inv = 1.0f / _mass;
}
// Enforce positive value
if (_mass_est_inv < FLT_EPSILON) {
_mass_est_inv = FLT_EPSILON;
}
if ((1.0f / _mass_est_inv) < FLT_EPSILON) {
_mass_est_inv = 1.0f / _mass;
}
}
// Publish estimated model
if ((now - _prev_publish_time) > (1e6f / _param_vm_est_pubrate.get())) {
_prev_publish_time = now;
// Compute inertia matrix and mass
Matrix3f I = _I;
float mass = _mass;
if (_param_vm_est_en.get()) {
// Invert mass estimation
mass = 1.0f / _mass_est_inv;
// Try to invert inertia estimation
bool invertible = inv(_I_est_inv, I);
if (not invertible) {
// Fallback to default value
I = _I;
_I_est_inv = inv(_I);
}
} else {
// reset estimation
_I_est_inv = inv(_I);
_mass_est_inv = 1.0f / _mass;
}
// Publish
vehicle_model_s vehicle_model;
vehicle_model.timestamp = hrt_absolute_time();
vehicle_model.mass = mass;
I.copyTo(vehicle_model.inertia);
_vehicle_model_pub.publish(vehicle_model);
}
perf_end(_loop_perf);
}
int VehicleModelEstimator::task_spawn(int argc, char *argv[])
{
VehicleModelEstimator *instance = new VehicleModelEstimator();
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 VehicleModelEstimator::print_status()
{
PX4_INFO("Running");
perf_print_counter(_loop_perf);
return 0;
}
int VehicleModelEstimator::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int VehicleModelEstimator::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements the estimation of vehicle model (mass & inertia).
To estimate the vehicle inertia, the estimator low-pass filters and
computes the time derivative of the vehicle angular acceleration
and of the vehicle torque setpoint, then applies a least-mean-square.
Similarly, to estimate the vehicle mass, the estimator low-pass
filters and computes the time derivative of the vehicle acceleration
and of the vehicle thrust setpoint, then applies a least-mean-square.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
/**
* Multicopter rate control app start / stop handling function
*/
extern "C" __EXPORT int vehicle_model_estimator_main(int argc, char *argv[]);
int vehicle_model_estimator_main(int argc, char *argv[])
{
return VehicleModelEstimator::main(argc, argv);
}

View File

@ -0,0 +1,188 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file VehicleModelEstimator.hpp
*
* Vehicle model estimator.
*
* To estimate the vehicle inertia, the estimator low-pass filters and
* computes the time derivative of the vehicle angular acceleration
* and of the vehicle torque setpoint, then applies a least-mean-square.
* Similarly, to estimate the vehicle mass, the estimator low-pass
* filters and computes the time derivative of the vehicle acceleration
* and of the vehicle thrust setpoint, then applies a least-mean-square.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/ecl/EKF/ekf.h> // included for RingBuffer
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_model.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
class VehicleModelEstimator : public ModuleBase<VehicleModelEstimator>, public ModuleParams, public px4::WorkItem
{
public:
VehicleModelEstimator();
virtual ~VehicleModelEstimator();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
void Run() override;
bool init();
private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */
uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_angular_acceleration_sub{this, ORB_ID(vehicle_angular_acceleration)};
uORB::SubscriptionCallbackWorkItem _vehicle_acceleration_sub{this, ORB_ID(vehicle_acceleration)};
uORB::Publication<vehicle_model_s> _vehicle_model_pub{ORB_ID(vehicle_model)}; /**< vehicle model publication */
bool _armed{false};
bool _landed{true};
bool _maybe_landed{true};
perf_counter_t _loop_perf; /**< loop duration performance counter */
static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
hrt_abstime _task_start{hrt_absolute_time()};
hrt_abstime _prev_publish_time{0};
// Inertia estimation data
matrix::SquareMatrix3f _I; /**< Inertia matrix entered via parameters */
matrix::SquareMatrix3f _I_est_inv; /**< Inverse of estimated inertia matrix */
// Angular acceleration data
math::LowPassFilter2pVector3f _lpf_ang_accel{0.f, 0.f}; /**< low-pass filters for angular acceleration */
matrix::Vector3f _ang_accel_filtered; /**< low-pass filtered angular acceleration */
matrix::Vector3f _ang_accel_filtered_diff; /**< time derivative of the low-pass filtered angular acceleration */
hrt_abstime _timestamp_ang_accel{0};
float _loop_update_rate_hz_ang_accel{initial_update_rate_hz};
int _loop_counter_ang_accel{0};
float _dt_accumulator_ang_accel{0.0f};
// Torque setpoint data
RingBuffer<vehicle_torque_setpoint_s> _torque_sp_buffer;
math::LowPassFilter2pVector3f _lpf_torque_sp{0.f, 0.f}; /**< low-pass filters for torque setpoint */
matrix::Vector3f _torque_sp_filtered; /**< low-pass filtered torque setpoint */
matrix::Vector3f _torque_sp_filtered_diff; /**< time derivative of the low-pass filtered torque setpoint */
hrt_abstime _timestamp_torque_sp{0};
float _loop_update_rate_hz_torque_sp{initial_update_rate_hz};
int _loop_counter_torque_sp{0};
float _dt_accumulator_torque_sp{0.0f};
// Mass estimation data
float _mass{0.0f}; /**< Mass entered via parameters */
float _mass_est_inv{1.0f}; /**< Inverse of estimated mass */
// Linear acceleration data
math::LowPassFilter2pVector3f _lpf_lin_accel{0.f, 0.f}; /**< low-pass filters for linear acceleration */
matrix::Vector3f _lin_accel_filtered; /**< low-pass filtered linear acceleration */
matrix::Vector3f _lin_accel_filtered_diff; /**< time derivative of the low-pass filtered linear acceleration */
hrt_abstime _timestamp_lin_accel{0};
float _loop_update_rate_hz_lin_accel{initial_update_rate_hz};
int _loop_counter_lin_accel{0};
float _dt_accumulator_lin_accel{0.0f};
// Thrust setpoint data
RingBuffer<vehicle_thrust_setpoint_s> _thrust_sp_buffer;
math::LowPassFilter2pVector3f _lpf_thrust_sp{0.f, 0.f}; /**< low-pass filters for thrust setpoint */
matrix::Vector3f _thrust_sp_filtered; /**< low-pass filtered thrust setpoint */
matrix::Vector3f _thrust_sp_filtered_diff; /**< time derivative of the low-pass filtered thrust setpoint */
hrt_abstime _timestamp_thrust_sp{0};
float _loop_update_rate_hz_thrust_sp{initial_update_rate_hz};
int _loop_counter_thrust_sp{0};
float _dt_accumulator_thrust_sp{0.0f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::VM_MASS>) _param_vm_mass, /**< vehicle mass */
(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx, /**< vehicle inertia */
(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
(ParamInt<px4::params::VM_EST_EN>) _param_vm_est_en,
(ParamFloat<px4::params::VM_EST_PUBRATE>) _param_vm_est_pubrate,
(ParamFloat<px4::params::VM_EST_CUTOFF>) _param_vm_est_cutoff,
(ParamFloat<px4::params::VM_EST_GAIN>) _param_vm_est_gain,
(ParamFloat<px4::params::VM_EST_DELAY>) _param_vm_est_delay
)
};

View File

@ -0,0 +1,111 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_model_estimator_params.c
* Parameters for the estimation of the vehicle model.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Enables the vehicle model estimation
*
* @boolean
* @group Vehicle Model
*/
PARAM_DEFINE_INT32(VM_EST_EN, 1);
/**
* Estimated vehicle model publishing rate
*
* The estimator will always run at maximum rate, updating its
* estimate at full rate.
* This parameter may be used to limit the rate at which the
* resulting estimation is published.
*
* @unit Hz
* @min 0
* @max 1000
* @decimal 0
* @increment 1
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_EST_PUBRATE, 10.0f);
/**
* Estimation gain
*
* A larger value will lead to faster update of the estimated model
* but an excessively large value may cause faulty estimation.
*
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_EST_GAIN, 0.1f);
/**
* Vehicle model estimator cutoff frequency
*
* Determines the cutoff frequency of the 2nd order low pass filter
* applied on control setpoints and sensor data before estimation.
*
* @min 0
* @max 30
* @decimal 2
* @increment 1
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_EST_CUTOFF, 5.0f);
/**
* Vehicle model estimator actuator delay
*
* Determines the delay applied to the torque and thrust setpoints
* in order to take into account the time required for the
* setpoints to take effect.
* This delay may vary depending on
* - whether the setpoints go through an IO board
* - the latency of the protocol used to communicate with actuators
* - the intrinsic latency of actuators
*
* @unit s
* @min 0
* @max 0.5
* @decimal 4
* @increment 1
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_EST_DELAY, 0.080f);

View File

@ -0,0 +1,109 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_model_params.c
* Parameters for vehicle model.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Mass
*
* @unit kg
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
/**
* Inertia matrix, XX component
*
* @unit kg.m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 1.f);
/**
* Inertia matrix, YY component
*
* @unit kg.m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 1.f);
/**
* Inertia matrix, ZZ component
*
* @unit kg.m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 1.f);
/**
* Inertia matrix, XY component
*
* @unit kg.m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
/**
* Inertia matrix, XZ component
*
* @unit kg.m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
/**
* Inertia matrix, YZ component
*
* @unit kg.m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);