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

delete control_state and cleanup vehicle_attitude (#7882)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-21 16:24:53 -04:00
committed by GitHub
parent 5bea264a5f
commit b4755297ec
42 changed files with 776 additions and 1061 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+48 -22
View File
@@ -71,13 +71,14 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -129,7 +130,7 @@ private:
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _ctrl_state_sub; /**< control state subscription */
int _v_att_sub; /**< vehicle attitude subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
@@ -137,10 +138,11 @@ private:
int _manual_control_sp_sub; /**< manual control setpoint subscription */
int _armed_sub; /**< arming status subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _motor_limits_sub; /**< motor limits subscription */
int _battery_status_sub; /**< battery status subscription */
int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */
int _sensor_correction_sub; /**< sensor thermal correction subscription */
int _motor_limits_sub; /**< motor limits subscription */
int _battery_status_sub; /**< battery status subscription */
int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */
int _sensor_correction_sub; /**< sensor thermal correction subscription */
int _sensor_bias_sub; /**< sensor in-run bias correction subscription */
unsigned _gyro_count;
int _selected_gyro;
@@ -154,7 +156,7 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@@ -166,6 +168,7 @@ private:
struct battery_status_s _battery_status; /**< battery status */
struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */
struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */
struct sensor_bias_s _sensor_bias; /**< sensor in-run bias corrections */
MultirotorMixer::saturation_status _saturation_status{};
@@ -339,13 +342,18 @@ private:
/**
* Check for control state updates.
*/
void control_state_poll();
void vehicle_attitude_poll();
/**
* Check for sensor thermal correction updates.
*/
void sensor_correction_poll();
/**
* Check for sensor in-run bias correction updates.
*/
void sensor_bias_poll();
/**
* Shim for calling task_main from task_create.
*/
@@ -369,7 +377,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_control_task(-1),
/* subscriptions */
_ctrl_state_sub(-1),
_v_att_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
@@ -379,6 +387,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_motor_limits_sub(-1),
_battery_status_sub(-1),
_sensor_correction_sub(-1),
_sensor_bias_sub(-1),
/* gyro selection */
_gyro_count(1),
@@ -393,7 +402,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_actuators_0_circuit_breaker_enabled(false),
_ctrl_state{},
_v_att{},
_v_att_sp{},
_v_rates_sp{},
_manual_control_sp{},
@@ -405,6 +414,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_battery_status{},
_sensor_gyro{},
_sensor_correction{},
_sensor_bias{},
_saturation_status{},
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
@@ -787,14 +798,14 @@ MulticopterAttitudeControl::battery_status_poll()
}
void
MulticopterAttitudeControl::control_state_poll()
MulticopterAttitudeControl::vehicle_attitude_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_ctrl_state_sub, &updated);
orb_check(_v_att_sub, &updated);
if (updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
}
}
@@ -815,6 +826,19 @@ MulticopterAttitudeControl::sensor_correction_poll()
}
}
void
MulticopterAttitudeControl::sensor_bias_poll()
{
/* check if there is a new message */
bool updated;
orb_check(_sensor_bias_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias);
}
}
/**
* Attitude controller.
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -832,7 +856,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
math::Matrix<3, 3> R_sp = q_sp.to_dcm();
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Quaternion q_att(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
@@ -995,9 +1019,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
rates = _board_rotation * rates;
// correct for in-run bias errors
rates(0) -= _ctrl_state.roll_rate_bias;
rates(1) -= _ctrl_state.pitch_rate_bias;
rates(2) -= _ctrl_state.yaw_rate_bias;
rates(0) -= _sensor_bias.gyro_x_bias;
rates(1) -= _sensor_bias.gyro_y_bias;
rates(2) -= _sensor_bias.gyro_z_bias;
math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
//math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
@@ -1071,9 +1095,9 @@ MulticopterAttitudeControl::task_main()
/*
* do subscriptions
*/
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1093,6 +1117,7 @@ MulticopterAttitudeControl::task_main()
}
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
/* initialize parameters cache */
parameters_update();
@@ -1148,8 +1173,9 @@ MulticopterAttitudeControl::task_main()
vehicle_status_poll();
vehicle_motor_limits_poll();
battery_status_poll();
control_state_poll();
vehicle_attitude_poll();
sensor_correction_poll();
sensor_bias_poll();
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal att control). If both are true don't
@@ -1171,7 +1197,7 @@ MulticopterAttitudeControl::task_main()
} else {
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Quaternion q(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]);
math::Quaternion q_sp(&_v_att_sp.q_d[0]);
_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);
@@ -1240,7 +1266,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.control[7] = _v_att_sp.landing_gear;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators.timestamp_sample = _v_att.timestamp;
/* scale effort by battery status */
if (_params.bat_scale_en && _battery_status.scale > 0.0f) {
@@ -1290,7 +1316,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[2] = 0.0f;
_actuators.control[3] = 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators.timestamp_sample = _v_att.timestamp;
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {