fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator move to new uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar 2019-06-03 20:19:56 -04:00
parent 79eb74be3f
commit e4ad994763
8 changed files with 55 additions and 173 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/navigator/follow_target.cpp
View File

@ -90,10 +90,6 @@ void FollowTarget::on_activation()
}
_rot_matrix = (_follow_position_matricies[_follow_target_position]);
if (_follow_target_sub < 0) {
_follow_target_sub = orb_subscribe(ORB_ID(follow_target));
}
}
void FollowTarget::on_active()
@ -106,9 +102,7 @@ void FollowTarget::on_active()
bool updated = false;
float dt_ms = 0;
orb_check(_follow_target_sub, &updated);
if (updated) {
if (_follow_target_sub.updated()) {
follow_target_s target_motion;
_target_updates++;
@ -117,7 +111,7 @@ void FollowTarget::on_active()
_previous_target_motion = _current_target_motion;
orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion);
_follow_target_sub.copy(&target_motion);
if (_current_target_motion.timestamp == 0) {
_current_target_motion = target_motion;

3
src/modules/navigator/follow_target.h
View File

@ -47,6 +47,7 @@
#include <matrix/math.hpp>
#include <px4_module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/follow_target.h>
class FollowTarget : public MissionBlock, public ModuleParams
@ -99,7 +100,7 @@ private:
FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION};
int _follow_target_position{FOLLOW_FROM_BEHIND};
int _follow_target_sub{-1};
uORB::Subscription _follow_target_sub{ORB_ID(follow_target)};
float _step_time_in_ms{0.0f};
float _follow_offset{OFFSET_M};

14
src/modules/navigator/mission.cpp
View File

@ -79,10 +79,7 @@ Mission::on_inactive()
}
if (_inited) {
bool mission_sub_updated = false;
orb_check(_navigator->get_mission_sub(), &mission_sub_updated);
if (mission_sub_updated) {
if (_mission_sub.updated()) {
update_mission();
if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
@ -180,8 +177,7 @@ Mission::on_active()
check_mission_valid(false);
/* check if anything has changed */
bool mission_sub_updated = false;
orb_check(_navigator->get_mission_sub(), &mission_sub_updated);
bool mission_sub_updated = _mission_sub.updated();
if (mission_sub_updated) {
update_mission();
@ -453,9 +449,9 @@ Mission::update_mission()
* an existing ROI setting from previous missions */
_navigator->reset_vroi();
struct mission_s old_mission = _mission;
const mission_s old_mission = _mission;
if (orb_copy(ORB_ID(mission), _navigator->get_mission_sub(), &_mission) == OK) {
if (_mission_sub.copy(&_mission)) {
/* determine current index */
if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
_current_mission_index = _mission.current_seq;
@ -499,7 +495,7 @@ Mission::update_mission()
}
} else {
PX4_ERR("mission update failed, handle: %d", _navigator->get_mission_sub());
PX4_ERR("mission update failed");
}
if (failed) {

4
src/modules/navigator/mission.h
View File

@ -53,6 +53,7 @@
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <px4_module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
@ -242,7 +243,8 @@ private:
(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
)
struct mission_s _mission {};
uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */
mission_s _mission {};
int32_t _current_mission_index{-1};

36
src/modules/navigator/navigator.h
View File

@ -62,16 +62,21 @@
#include <px4_module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
/**
@ -164,8 +169,6 @@ public:
bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos); }
int get_mission_sub() { return _mission_sub; }
Geofence &get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
@ -312,17 +315,19 @@ private:
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err
)
int _global_pos_sub{-1}; /**< global position subscription */
int _gps_pos_sub{-1}; /**< gps position subscription */
int _home_pos_sub{-1}; /**< home position subscription */
int _land_detected_sub{-1}; /**< vehicle land detected subscription */
int _local_pos_sub{-1}; /**< local position subscription */
int _mission_sub{-1}; /**< mission subscription */
int _param_update_sub{-1}; /**< param update subscription */
int _pos_ctrl_landing_status_sub{-1}; /**< position controller landing status subscription */
int _traffic_sub{-1}; /**< traffic subscription */
int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */
int _vstatus_sub{-1}; /**< vehicle status subscription */
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; /**< param update subscription */
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
uORB::Subscription _vstatus_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
orb_advert_t _geofence_result_pub{nullptr};
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
@ -341,8 +346,6 @@ private:
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_status_s _vstatus{}; /**< vehicle status */
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/
// Publications
@ -387,12 +390,7 @@ private:
float _mission_throttle{-1.0f};
// update subscriptions
void global_position_update();
void gps_position_update();
void home_position_update(bool force = false);
void local_position_update();
void params_update();
void vehicle_land_detected_update();
void vehicle_status_update();
/**

148
src/modules/navigator/navigator_main.cpp
View File

@ -57,14 +57,6 @@
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
/**
* navigator app start / stop handling function
@ -114,85 +106,31 @@ Navigator::Navigator() :
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
_handle_reverse_delay = param_find("VT_B_REV_DEL");
// subscriptions
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_pos_ctrl_landing_status_sub = orb_subscribe(ORB_ID(position_controller_landing_status));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_mission_sub = orb_subscribe(ORB_ID(mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_traffic_sub = orb_subscribe(ORB_ID(transponder_report));
reset_triplets();
}
Navigator::~Navigator()
{
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_gps_pos_sub);
orb_unsubscribe(_pos_ctrl_landing_status_sub);
orb_unsubscribe(_vstatus_sub);
orb_unsubscribe(_land_detected_sub);
orb_unsubscribe(_home_pos_sub);
orb_unsubscribe(_mission_sub);
orb_unsubscribe(_param_update_sub);
orb_unsubscribe(_vehicle_command_sub);
}
void
Navigator::global_position_update()
{
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
void
Navigator::local_position_update()
{
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
void
Navigator::gps_position_update()
{
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
}
void
Navigator::home_position_update(bool force)
{
bool updated = false;
orb_check(_home_pos_sub, &updated);
if (updated || force) {
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
}
}
void
Navigator::vehicle_status_update()
{
if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
if (_vstatus_sub.update(&_vstatus)) {
/* in case the commander is not be running */
_vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
}
void
Navigator::vehicle_land_detected_update()
{
orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
}
void
Navigator::params_update()
{
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
_param_update_sub.update(&param_update);
updateParams();
if (_handle_back_trans_dec_mss != PARAM_INVALID) {
@ -220,11 +158,6 @@ Navigator::run()
/* copy all topics first time */
vehicle_status_update();
vehicle_land_detected_update();
global_position_update();
local_position_update();
gps_position_update();
home_position_update(true);
params_update();
/* wakeup source(s) */
@ -256,19 +189,15 @@ Navigator::run()
} else {
if (fds[0].revents & POLLIN) {
/* success, local pos is available */
local_position_update();
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
}
perf_begin(_loop_perf);
bool updated;
/* gps updated */
orb_check(_gps_pos_sub, &updated);
if (updated) {
gps_position_update();
if (_gps_pos_sub.updated()) {
_gps_pos_sub.copy(&_gps_pos);
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
@ -276,10 +205,8 @@ Navigator::run()
}
/* global position updated */
orb_check(_global_pos_sub, &updated);
if (updated) {
global_position_update();
if (_global_pos_sub.updated()) {
_global_pos_sub.copy(&_global_pos);
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
@ -287,42 +214,18 @@ Navigator::run()
}
/* parameters updated */
orb_check(_param_update_sub, &updated);
if (updated) {
if (_param_update_sub.updated()) {
params_update();
}
/* vehicle status updated */
orb_check(_vstatus_sub, &updated);
if (updated) {
vehicle_status_update();
}
/* vehicle land detected updated */
orb_check(_land_detected_sub, &updated);
if (updated) {
vehicle_land_detected_update();
}
/* navigation capabilities updated */
vehicle_status_update();
_land_detected_sub.update(&_land_detected);
_position_controller_status_sub.update();
_home_pos_sub.update(&_home_pos);
/* home position updated */
orb_check(_home_pos_sub, &updated);
if (updated) {
home_position_update();
}
/* vehicle_command updated */
orb_check(_vehicle_command_sub, &updated);
if (updated) {
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (_vehicle_command_sub.updated()) {
vehicle_command_s cmd{};
_vehicle_command_sub.copy(&cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
@ -1054,23 +957,22 @@ void Navigator::check_traffic()
// float vel_e = get_global_position()->vel_e;
// float vel_d = get_global_position()->vel_d;
bool changed;
orb_check(_traffic_sub, &changed);
bool changed = _traffic_sub.updated();
float horizontal_separation = 500;
float vertical_separation = 500;
while (changed) {
transponder_report_s tr;
orb_copy(ORB_ID(transponder_report), _traffic_sub, &tr);
transponder_report_s tr{};
_traffic_sub.copy(&tr);
uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
if ((tr.flags & required_flags) != required_flags) {
orb_check(_traffic_sub, &changed);
changed = _traffic_sub.updated();
continue;
}
@ -1145,7 +1047,7 @@ void Navigator::check_traffic()
}
}
orb_check(_traffic_sub, &changed);
changed = _traffic_sub.updated();
}
}
@ -1158,15 +1060,11 @@ Navigator::abort_landing()
if (_pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
bool updated = false;
orb_check(_pos_ctrl_landing_status_sub, &updated);
if (updated) {
position_controller_landing_status_s landing_status = {};
if (_pos_ctrl_landing_status_sub.updated()) {
position_controller_landing_status_s landing_status{};
// landing status from position controller must be newer than navigator's last position setpoint
if (orb_copy(ORB_ID(position_controller_landing_status), _pos_ctrl_landing_status_sub, &landing_status) == PX4_OK) {
if (_pos_ctrl_landing_status_sub.copy(&landing_status)) {
if (landing_status.timestamp > _pos_sp_triplet.timestamp) {
should_abort = landing_status.abort_landing;
}

10
src/modules/navigator/precland.cpp
View File

@ -72,11 +72,6 @@ PrecLand::PrecLand(Navigator *navigator) :
void
PrecLand::on_activation()
{
// We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned
if (_target_pose_sub < 0) {
_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose));
}
_state = PrecLandState::Start;
_search_cnt = 0;
_last_slewrate_time = 0;
@ -111,10 +106,7 @@ void
PrecLand::on_active()
{
// get new target measurement
orb_check(_target_pose_sub, &_target_pose_updated);
if (_target_pose_updated) {
orb_copy(ORB_ID(landing_target_pose), _target_pose_sub, &_target_pose);
if (_target_pose_sub.update(&_target_pose)) {
_target_pose_valid = true;
}

3
src/modules/navigator/precland.h
View File

@ -43,6 +43,7 @@
#include <matrix/math.hpp>
#include <lib/ecl/geo/geo.h>
#include <px4_module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/landing_target_pose.h>
#include "navigator_mode.h"
@ -103,7 +104,7 @@ private:
landing_target_pose_s _target_pose{}; /**< precision landing target position */
int _target_pose_sub{-1};
uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};
bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
bool _target_pose_updated{false}; /**< wether the landing target position message is updated */