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

fw_pos_control_l1 move to new uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-06-06 20:00:02 -04:00
parent 528d2f61a0
commit 4bef573497
2 changed files with 38 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+26 -101
View File
@@ -297,14 +297,10 @@ FixedwingPositionControl::parameters_update()
void
FixedwingPositionControl::vehicle_control_mode_poll()
{
bool updated = false;
orb_check(_control_mode_sub, &updated);
if (updated) {
if (_control_mode_sub.updated()) {
const bool was_armed = _control_mode.flag_armed;
if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) == PX4_OK) {
if (_control_mode_sub.copy(&_control_mode)) {
// reset state when arming
if (!was_armed && _control_mode.flag_armed) {
@@ -318,12 +314,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
void
FixedwingPositionControl::vehicle_command_poll()
{
bool updated;
orb_check(_vehicle_command_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command);
if (_vehicle_command_sub.updated()) {
_vehicle_command_sub.copy(&_vehicle_command);
handle_command();
}
}
@@ -331,13 +323,7 @@ FixedwingPositionControl::vehicle_command_poll()
void
FixedwingPositionControl::vehicle_status_poll()
{
bool updated;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
if (_vehicle_status_sub.update(&_vehicle_status)) {
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (_attitude_setpoint_id == nullptr) {
if (_vehicle_status.is_vtol) {
@@ -355,31 +341,6 @@ FixedwingPositionControl::vehicle_status_poll()
}
}
void
FixedwingPositionControl::vehicle_land_detected_poll()
{
bool updated;
orb_check(_vehicle_land_detected_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
}
}
void
FixedwingPositionControl::manual_control_setpoint_poll()
{
bool manual_updated;
/* Check if manual setpoint has changed */
orb_check(_manual_control_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
}
}
void
FixedwingPositionControl::airspeed_poll()
{
@@ -419,39 +380,21 @@ FixedwingPositionControl::airspeed_poll()
void
FixedwingPositionControl::vehicle_attitude_poll()
{
/* check if there is a new position */
bool updated;
orb_check(_vehicle_attitude_sub, &updated);
if (_vehicle_attitude_sub.update(&_att)) {
/* set rotation matrix and euler angles */
_R_nb = Quatf(_att.q);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
}
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
// between multirotor and fixed wing flight
if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
_R_nb = _R_nb * R_offset;
}
/* set rotation matrix and euler angles */
_R_nb = Quatf(_att.q);
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
// between multirotor and fixed wing flight
if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
_R_nb = _R_nb * R_offset;
}
Eulerf euler_angles(_R_nb);
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
}
void
FixedwingPositionControl::position_setpoint_triplet_poll()
{
/* check if there is a new setpoint */
bool pos_sp_triplet_updated;
orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
if (pos_sp_triplet_updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
Eulerf euler_angles(_R_nb);
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
}
}
@@ -1732,20 +1675,7 @@ FixedwingPositionControl::handle_command()
void
FixedwingPositionControl::run()
{
/*
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@@ -1780,13 +1710,12 @@ FixedwingPositionControl::run()
}
/* only update parameters if they changed */
bool params_updated = false;
orb_check(_params_sub, &params_updated);
bool params_updated = _params_sub.updated();
if (params_updated) {
/* read from param to clear updated flag */
parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
_params_sub.copy(&update);
/* update parameters from storage */
parameters_update();
@@ -1798,7 +1727,7 @@ FixedwingPositionControl::run()
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
_local_pos_sub.update(&_local_pos);
// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
@@ -1823,12 +1752,12 @@ FixedwingPositionControl::run()
_sub_sensors.update();
airspeed_poll();
manual_control_setpoint_poll();
position_setpoint_triplet_poll();
_manual_control_sub.update(&_manual);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
vehicle_attitude_poll();
vehicle_command_poll();
vehicle_control_mode_poll();
vehicle_land_detected_poll();
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
vehicle_status_poll();
Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
@@ -2023,13 +1952,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
/* scale throttle cruise by baro pressure */
if (_parameters.throttle_alt_scale > FLT_EPSILON) {
sensor_baro_s baro{};
bool baro_updated = false;
orb_check(_sensor_baro_sub, &baro_updated);
sensor_baro_s baro;
if (orb_copy(ORB_ID(sensor_baro), _sensor_baro_sub, &baro) == PX4_OK) {
if (_sensor_baro_sub.update(&baro)) {
if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_parameters.throttle_alt_scale)) {
// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_MBAR / baro.pressure);
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+12 -14
View File
@@ -152,16 +152,17 @@ private:
orb_advert_t _mavlink_log_pub{nullptr};
int _global_pos_sub{-1};
int _local_pos_sub{-1};
int _pos_sp_triplet_sub{-1};
int _control_mode_sub{-1}; ///< control mode subscription */
int _vehicle_attitude_sub{-1}; ///< vehicle attitude subscription */
int _vehicle_command_sub{-1}; ///< vehicle command subscription */
int _vehicle_status_sub{-1}; ///< vehicle status subscription */
int _vehicle_land_detected_sub{-1}; ///< vehicle land detected subscription */
int _params_sub{-1}; ///< notification of parameter updates */
int _manual_control_sub{-1}; ///< notification of manual control updates */
int _sensor_baro_sub{-1};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _manual_control_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates */
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates */
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; ///< vehicle attitude subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; ///< vehicle command subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; ///< vehicle land detected subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription */
orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint */
orb_advert_t _pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */
@@ -172,7 +173,7 @@ private:
manual_control_setpoint_s _manual {}; ///< r/c channel data */
position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items */
vehicle_attitude_s _att {}; ///< vehicle attitude setpoint */
vehicle_attitude_s _att {}; ///< vehicle attitude setpoint */
vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint */
vehicle_command_s _vehicle_command {}; ///< vehicle commands */
vehicle_control_mode_s _control_mode {}; ///< control mode */
@@ -377,12 +378,9 @@ private:
// Update subscriptions
void airspeed_poll();
void control_update();
void manual_control_setpoint_poll();
void position_setpoint_triplet_poll();
void vehicle_attitude_poll();
void vehicle_command_poll();
void vehicle_control_mode_poll();
void vehicle_land_detected_poll();
void vehicle_status_poll();
void status_publish();