dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

This commit is contained in:
Julian Oes 2014-06-19 10:37:59 +02:00
parent c23bc93681 5bab3b1bac
commit ad7e0ee17f
2 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -58,6 +58,12 @@
#include "rtl.h" #include "rtl.h"
#include "geofence.h" #include "geofence.h"
/**
* Number of navigation modes that need on_active/on_inactive calls
* Currently: mission, loiter, and rtl
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 3
class Navigator : public control::SuperBlock class Navigator : public control::SuperBlock
{ {
public: public:
@ -153,6 +159,8 @@ private:
Loiter _loiter; /**< class that handles loiter */ Loiter _loiter; /**< class that handles loiter */
RTL _rtl; /**< class that handles RTL */ RTL _rtl; /**< class that handles RTL */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _is_in_loiter; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */ bool _update_triplet; /**< flags if position SP triplet needs to be published */

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

@ -125,6 +125,11 @@ Navigator::Navigator() :
_param_loiter_radius(this, "LOITER_RAD"), _param_loiter_radius(this, "LOITER_RAD"),
_param_takeoff_acceptance_radius(this, "TF_ACC_RAD") _param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
{ {
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
updateParams(); updateParams();
} }
@ -364,23 +369,13 @@ Navigator::task_main()
break; break;
} }
/* TODO: make list of modes and loop through it */ /* iterate through navigation modes and set active/inactive for each */
if (_navigation_mode == &_mission) { for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_update_triplet = _mission.on_active(&_pos_sp_triplet); if (_navigation_mode == _navigation_mode_array[i]) {
} else { _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet);
_mission.on_inactive(); } else {
} _navigation_mode_array[i]->on_inactive();
}
if (_navigation_mode == &_rtl) {
_update_triplet = _rtl.on_active(&_pos_sp_triplet);
} else {
_rtl.on_inactive();
}
if (_navigation_mode == &_loiter) {
_update_triplet = _loiter.on_active(&_pos_sp_triplet);
} else {
_loiter.on_inactive();
} }
/* if nothing is running, set position setpoint triplet invalid */ /* if nothing is running, set position setpoint triplet invalid */