fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 10:53:34 +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/gnd_att_control/GroundRoverAttitudeControl.cpp
+6 -6
View File
@@ -184,7 +184,7 @@ void
GroundRoverAttitudeControl::task_main()
{
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -204,7 +204,7 @@ GroundRoverAttitudeControl::task_main()
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _ctrl_state_sub;
fds[1].fd = _att_sub;
fds[1].events = POLLIN;
_task_running = true;
@@ -252,7 +252,7 @@ GroundRoverAttitudeControl::task_main()
}
/* load local copies */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
@@ -264,10 +264,10 @@ GroundRoverAttitudeControl::task_main()
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
Eulerf euler_angles(matrix::Quatf(_ctrl_state.q));
Eulerf euler_angles(matrix::Quatf(_att.q));
/* Calculate the control output for the steering as yaw */
float yaw_u = pid_calculate(&_steering_ctrl, _att_sp.yaw_body, euler_angles.psi(), _ctrl_state.yaw_rate, deltaT);
float yaw_u = pid_calculate(&_steering_ctrl, _att_sp.yaw_body, euler_angles.psi(), _att.yawspeed, deltaT);
float angle_diff = 0.0f;
@@ -322,7 +322,7 @@ GroundRoverAttitudeControl::task_main()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators.timestamp_sample = _att.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_attitude_enabled ||
src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp
+3 -3
View File
@@ -53,9 +53,9 @@
#include <systemlib/perf_counter.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/parameter_update.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>
@@ -81,7 +81,7 @@ private:
int _att_sp_sub{-1}; /**< vehicle attitude setpoint */
int _battery_status_sub{-1}; /**< battery status subscription */
int _ctrl_state_sub{-1}; /**< control state subscription */
int _att_sub{-1}; /**< control state subscription */
int _manual_sub{-1}; /**< notification of manual control updates */
int _params_sub{-1}; /**< notification of parameter updates */
int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */
@@ -90,8 +90,8 @@ private:
actuator_controls_s _actuators {}; /**< actuator control inputs */
battery_status_s _battery_status {}; /**< battery status */
control_state_s _ctrl_state {}; /**< control state */
manual_control_setpoint_s _manual {}; /**< r/c channel data */
vehicle_attitude_s _att {}; /**< control state */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */