diff options
Diffstat (limited to 'src/modules/controllib/uorb')
-rw-r--r-- | src/modules/controllib/uorb/blocks.cpp | 18 | ||||
-rw-r--r-- | src/modules/controllib/uorb/blocks.hpp | 9 |
2 files changed, 14 insertions, 13 deletions
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 448a42a99..e213ac17f 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) + mission_item_s &missionCmd, + mission_item_s &lastMissionCmd) { // heading to waypoint float psiTrack = get_bearing_to_next_waypoint( (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + missionCmd.lat, + missionCmd.lon); // cross track struct crosstrack_error_s xtrackError; get_distance_to_line(&xtrackError, (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + lastMissionCmd.lat, + lastMissionCmd.lon, + missionCmd.lat, + missionCmd.lon); _psiCmd = _wrap_2pi(psiTrack - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 46dc1bec2..335439fb7 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,8 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> @@ -82,8 +83,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); + mission_item_s &missionCmd, + mission_item_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +99,7 @@ protected: UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; + UOrbSubscription<mission_item_triplet_s> _missionCmd; UOrbSubscription<manual_control_setpoint_s> _manual; UOrbSubscription<vehicle_status_s> _status; UOrbSubscription<parameter_update_s> _param_update; |