Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fw_pos_control_l1 move to new uORB::Subscription #12185

Merged
merged 1 commit into from
Jun 7, 2019
Merged

fw_pos_control_l1 move to new uORB::Subscription #12185

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
129 changes: 27 additions & 102 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -318,26 +314,16 @@ 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();
}
}

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) {
Expand All @@ -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()
{
Expand Down Expand Up @@ -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 (updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
}

/* 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 (_vehicle_attitude_sub.update(&_att)) {
/* 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;
}

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);
}
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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();
Expand All @@ -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) {
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
26 changes: 12 additions & 14 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -152,16 +152,17 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
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 */
Expand All @@ -172,7 +173,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro

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 */
Expand Down Expand Up @@ -377,12 +378,9 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
// 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();
Expand Down