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

integrated ctrl state rates in mc_att_control

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-08 22:43:00 +02:00
parent 78f5a35d12
commit 6dbf4e4573
1 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+11 -4
View File
@@ -76,6 +76,7 @@
#include <uORB/topics/fw_virtual_rates_setpoint.h> #include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h> #include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
@@ -128,6 +129,7 @@ private:
int _control_task; /**< task handle */ int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */ int _v_control_mode_sub; /**< vehicle control mode subscription */
@@ -147,6 +149,7 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@@ -304,6 +307,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
/* subscriptions */ /* subscriptions */
_v_att_sub(-1), _v_att_sub(-1),
_ctrl_state_sub(-1),
_v_att_sp_sub(-1), _v_att_sp_sub(-1),
_v_control_mode_sub(-1), _v_control_mode_sub(-1),
_params_sub(-1), _params_sub(-1),
@@ -326,6 +330,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
{ {
memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att, 0, sizeof(_v_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
@@ -708,9 +713,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* current body angular rates */ /* current body angular rates */
math::Vector<3> rates; math::Vector<3> rates;
rates(0) = _v_att.rollspeed; rates(0) = _ctrl_state.roll_rate;
rates(1) = _v_att.pitchspeed; rates(1) = _ctrl_state.pitch_rate;
rates(2) = _v_att.yawspeed; rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */ /* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates; math::Vector<3> rates_err = _rates_sp - rates;
@@ -749,6 +754,7 @@ MulticopterAttitudeControl::task_main()
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -798,8 +804,9 @@ MulticopterAttitudeControl::task_main()
dt = 0.02f; dt = 0.02f;
} }
/* copy attitude topic */ /* copy attitude and control state topics */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* check for updates in other topics */ /* check for updates in other topics */
parameter_update_poll(); parameter_update_poll();