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

navigator move to new uORB::Subscription #12162

Merged
merged 1 commit into from
Jun 6, 2019
Merged

navigator move to new uORB::Subscription #12162

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
10 changes: 2 additions & 8 deletions src/modules/navigator/follow_target.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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++;
Expand All @@ -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;
Expand Down
3 changes: 2 additions & 1 deletion src/modules/navigator/follow_target.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -99,7 +100,7 @@ class FollowTarget : public MissionBlock, public ModuleParams
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};

Expand Down
14 changes: 5 additions & 9 deletions src/modules/navigator/mission.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down
4 changes: 3 additions & 1 deletion src/modules/navigator/mission.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -242,7 +243,8 @@ class Mission : public MissionBlock, public ModuleParams
(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};

Expand Down
36 changes: 17 additions & 19 deletions src/modules/navigator/navigator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>

/**
Expand Down Expand Up @@ -164,8 +169,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
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; }
Expand Down Expand Up @@ -312,17 +315,19 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
(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 */
Expand All @@ -341,8 +346,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
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
Expand Down Expand Up @@ -387,12 +390,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
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();

/**
Expand Down
Loading