From a401778038a38fc93bb2e97fdf52617e6652d643 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 1 Mar 2018 14:36:38 +0100 Subject: [PATCH] FlightTask: remove global to local map --- src/lib/FlightTasks/tasks/FlightTask.cpp | 8 -------- src/lib/FlightTasks/tasks/FlightTask.hpp | 13 ++++++------- src/lib/FlightTasks/tasks/FlightTaskAuto.cpp | 18 +++++++++++++++++- src/lib/FlightTasks/tasks/FlightTaskAuto.hpp | 5 +++++ 4 files changed, 28 insertions(+), 16 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp index e8ed481a93..413772a19b 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -61,14 +61,6 @@ bool FlightTask::_evaluateVehiclePosition() _velocity = matrix::Vector3f(&_sub_vehicle_local_position->get().vx); _yaw = _sub_vehicle_local_position->get().yaw; - /* Check if reference has changed and update. */ - if (_sub_vehicle_local_position->get().ref_timestamp != _time_stamp_reference) { - map_projection_init(&_reference_position, _sub_vehicle_local_position->get().ref_lat, - _sub_vehicle_local_position->get().ref_lon); - _reference_altitude = _sub_vehicle_local_position->get().ref_alt; - _time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp; - } - return true; } else { diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index bb0a824e54..5e45eb0f2b 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -114,7 +114,6 @@ protected: hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */ hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */ hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */ - hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update */ /* Current vehicle state */ matrix::Vector3f _position; /**< current vehicle position */ @@ -130,17 +129,17 @@ protected: float _yaw_setpoint; float _yawspeed_setpoint; - /* Current reference position */ - map_projection_reference_s _reference_position{}; /**< structure used to project lat/lon setpoint into local frame */ - float _reference_altitude = 0.0f; /**< altitude relative to ground */ - /** * Get the output data */ void _resetSetpoints(); -private: + /** + * Vehicle local position subscription + * TODO: Implement a message that is smaller than the + * current vehicle local position message + */ uORB::Subscription *_sub_vehicle_local_position{nullptr}; - bool _evaluateVehiclePosition(); + virtual bool _evaluateVehiclePosition(); }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index d8c1bdfd28..b14bc08d5e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -143,7 +143,7 @@ bool FlightTaskAuto::_evaluateTriplets() _type = (WaypointType)_sub_triplet_setpoint->get().current.type; - _prev_prev_wp = _prev_wp; // previous -1 is set to previsou + _prev_prev_wp = _prev_wp; // previous -1 is set to previous if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) { map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat, @@ -173,3 +173,19 @@ bool FlightTaskAuto::_isFinite(const position_setpoint_s sp) { return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt)); } + +bool FlightTaskAuto::_evaluateVehiclePosition() +{ + FlightTask::_evaluateVehiclePosition(); + + /* Check if reference has changed and update. */ + if (_sub_vehicle_local_position->get().ref_timestamp != _time_stamp_reference) { + map_projection_init(&_reference_position, + _sub_vehicle_local_position->get().ref_lat, + _sub_vehicle_local_position->get().ref_lon); + _reference_altitude = _sub_vehicle_local_position->get().ref_alt; + _time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp; + } + + return true; +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index 072d78bf04..674e568c4a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -82,8 +82,13 @@ private: control::BlockParamFloat _mc_cruise_default; /**< Default mc cruise speed*/ map_projection_reference_s _reference; /**< Reference frame from global to local */ uORB::Subscription *_sub_triplet_setpoint{nullptr}; + map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame */ + float _reference_altitude = 0.0f; /**< Altitude relative to ground */ + hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update */ bool _evaluateTriplets(); /**< Checks and sets triplets */ bool _isFinite(const position_setpoint_s sp); void _updateReference(); + + bool _evaluateVehiclePosition() override; /**< Required for reference update */ };