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

FlightTask: remove global to local map

Browse Source
This commit is contained in:
Dennis Mannhart 2018-03-01 14:36:38 +01:00 committed by Lorenz Meier
parent 0efbbdc227
commit a401778038
4 changed files with 28 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/FlightTasks/tasks/FlightTask.cpp
View File

@ -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 {

13
src/lib/FlightTasks/tasks/FlightTask.hpp
View File

@ -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<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
bool _evaluateVehiclePosition();
virtual bool _evaluateVehiclePosition();
};

18
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
View File

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

5
src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
View File

@ -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<position_setpoint_triplet_s> *_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 */
};