fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 05:33:35 +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_pos_control/GroundRoverPositionControl.cpp
+15 -16
View File
@@ -48,6 +48,7 @@ static int _control_task = -1; /**< task handle for sensor task */
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector3f;
/**
* L1 control app start / stop handling function
@@ -64,7 +65,9 @@ GroundRoverPositionControl *g_control = nullptr;
GroundRoverPositionControl::GroundRoverPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control"))
_sub_attitude(ORB_ID(vehicle_attitude), 0, 0, nullptr),
_sub_sensors(ORB_ID(sensor_bias), 0, 0, nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters
{
_parameter_handles.l1_period = param_find("GND_L1_PERIOD");
_parameter_handles.l1_damping = param_find("GND_L1_DAMPING");
@@ -178,17 +181,6 @@ GroundRoverPositionControl::manual_control_setpoint_poll()
}
}
void
GroundRoverPositionControl::control_state_poll()
{
bool ctrl_state_updated;
orb_check(_ctrl_state_sub, &ctrl_state_updated);
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
}
}
void
GroundRoverPositionControl::position_setpoint_triplet_poll()
{
@@ -260,9 +252,16 @@ GroundRoverPositionControl::control_position(const math::Vector<2> &current_posi
mission_target_speed = _pos_sp_triplet.current.cruising_speed;
}
//Compute airspeed control out and just scale it as a constant
// Velocity in body frame
const Dcmf R_to_body(Quatf(_sub_attitude.get().q).inversed());
const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
const float x_vel = vel(0);
const float x_acc = _sub_sensors.get().accel_x;
// Compute airspeed control out and just scale it as a constant
mission_throttle = _parameters.throttle_speed_scaler
* pid_calculate(&_speed_ctrl, mission_target_speed, _ctrl_state.x_vel, _ctrl_state.x_acc, dt);
* pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
// Constrain throttle between min and max
mission_throttle = math::constrain(mission_throttle, _parameters.throttle_min, _parameters.throttle_max);
@@ -331,7 +330,6 @@ void
GroundRoverPositionControl::task_main()
{
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -409,9 +407,10 @@ GroundRoverPositionControl::task_main()
// update the reset counters in any case
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
control_state_poll();
manual_control_setpoint_poll();
position_setpoint_triplet_poll();
_sub_attitude.update();
_sub_sensors.update();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp
+8 -4
View File
@@ -54,11 +54,13 @@
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/pid/pid.h>
#include <uORB/topics/control_state.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_bias.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_global_position.h>
@@ -66,6 +68,8 @@
using matrix::Dcmf;
using uORB::Subscription;
class GroundRoverPositionControl
{
public:
@@ -96,13 +100,11 @@ private:
bool _task_running{false}; /**< if true, task is running in its mainloop */
int _control_mode_sub{-1}; /**< control mode subscription */
int _ctrl_state_sub{-1}; /**< control state subscription */
int _global_pos_sub{-1};
int _manual_control_sub{-1}; /**< notification of manual control updates */
int _params_sub{-1}; /**< notification of parameter updates */
int _pos_sp_triplet_sub{-1};
control_state_s _ctrl_state{}; /**< control state */
fw_pos_ctrl_status_s _gnd_pos_ctrl_status{}; /**< navigation capabilities */
manual_control_setpoint_s _manual{}; /**< r/c channel data */
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
@@ -110,6 +112,9 @@ private:
vehicle_control_mode_s _control_mode{}; /**< control mode */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
Subscription<vehicle_attitude_s> _sub_attitude;
Subscription<sensor_bias_s> _sub_sensors;
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
@@ -180,7 +185,6 @@ private:
int parameters_update();
void manual_control_setpoint_poll();
void control_state_poll();
void position_setpoint_triplet_poll();
void vehicle_control_mode_poll();