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

navigator: NavigatorMode and MissionBlock API cleanup

This commit is contained in:
Anton Babushkin 2014-06-28 00:54:27 +02:00
parent affc312411
commit 456e628e12
11 changed files with 148 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/navigator/loiter.cpp
View File

@ -65,14 +65,22 @@ Loiter::~Loiter()
{
}
bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(pos_sp_triplet);
}
void
Loiter::on_inactive()
{
}
void
Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
{
set_loiter_item(pos_sp_triplet);
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
}
bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
return false;
}

14
src/modules/navigator/loiter.h
View File

@ -50,24 +50,14 @@
class Loiter : public MissionBlock
{
public:
/**
* Constructor
*/
Loiter(Navigator *navigator, const char *name);
/**
* Destructor
*/
~Loiter();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*/
virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
};

14
src/modules/navigator/mission.cpp
View File

@ -82,8 +82,6 @@ Mission::~Mission()
void
Mission::on_inactive()
{
_first_run = true;
/* check anyway if missions have changed so that feedback to groundstation is given */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@ -98,6 +96,12 @@ Mission::on_inactive()
}
}
void
Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
{
set_mission_items(pos_sp_triplet);
}
bool
Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
@ -117,10 +121,9 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated || _first_run) {
if (onboard_updated || offboard_updated) {
set_mission_items(pos_sp_triplet);
updated = true;
_first_run = false;
}
/* lets check if we reached the current mission item */
@ -255,6 +258,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
set_loiter_item(pos_sp_triplet);
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
reset_mission_item_reached();
report_mission_finished();
}

14
src/modules/navigator/mission.h
View File

@ -65,24 +65,14 @@ class Navigator;
class Mission : public MissionBlock
{
public:
/**
* Constructor
*/
Mission(Navigator *navigator, const char *name);
/**
* Destructor
*/
virtual ~Mission();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*/
virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
private:

65
src/modules/navigator/mission_block.cpp
View File

@ -45,6 +45,7 @@
#include <systemlib/err.h>
#include <geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <uORB/uORB.h>
@ -207,36 +208,44 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_
}
}
bool
void
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* don't change setpoint if 'can_loiter_at_sp' flag set */
if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
/* use current position */
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
if (_navigator->get_vstatus()->condition_landed) {
/* landed, don't takeoff, but switch to IDLE mode */
_mission_item.nav_cmd = NAV_CMD_IDLE;
_navigator->set_can_loiter_at_sp(true);
_navigator->set_can_loiter_at_sp(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE");
} else {
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
/* use current position setpoint */
_mission_item.lat = pos_sp_triplet->current.lat;
_mission_item.lon = pos_sp_triplet->current.lon;
_mission_item.altitude = pos_sp_triplet->current.alt;
} else {
/* use current position */
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
mavlink_log_info(_navigator->get_mavlink_fd(), "switch to LOITER");
}
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
|| pos_sp_triplet->next.valid) {
/* position setpoint triplet should be updated */
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
return true;
}
return false;
}

16
src/modules/navigator/mission_block.h
View File

@ -64,6 +64,7 @@ public:
*/
virtual ~MissionBlock();
protected:
/**
* Check if mission item has been reached
* @return true if successfully reached
@ -88,19 +89,16 @@ public:
void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
*
* @param the position setpoint triplet to set
* @return true if setpoint has changed
* Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
*/
bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;
// TODO remove argument, get position setpoint from navigator, add to arguments pointer to mission item instead
void set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet);
mission_item_s _mission_item;
bool _mission_item_valid;
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;
};
#endif

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

@ -371,11 +371,7 @@ Navigator::task_main()
/* iterate through navigation modes and set active/inactive for each */
for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
if (_navigation_mode == _navigation_mode_array[i]) {
_update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet);
} else {
_navigation_mode_array[i]->on_inactive();
}
_update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet);
}
/* if nothing is running, set position setpoint triplet invalid */

34
src/modules/navigator/navigator_mode.cpp
View File

@ -33,9 +33,10 @@
/**
* @file navigator_mode.cpp
*
* Helper class for different modes in navigator
* Base class for different modes in navigator
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "navigator_mode.h"
@ -55,16 +56,41 @@ NavigatorMode::~NavigatorMode()
{
}
bool
NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) {
if (active) {
if (_first_run) {
/* first run */
_first_run = false;
on_activation(pos_sp_triplet);
return true;
} else {
/* periodic updates when active */
on_active(pos_sp_triplet);
}
} else {
/* periodic updates when inactive */
_first_run = true;
on_inactive();
}
}
void
NavigatorMode::on_inactive()
{
_first_run = true;
}
void
NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* invalidate position setpoint by default */
pos_sp_triplet->current.valid = false;
}
bool
NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
pos_sp_triplet->current.valid = false;
_first_run = false;
return false;
}

12
src/modules/navigator/navigator_mode.h
View File

@ -33,9 +33,10 @@
/**
* @file navigator_mode.h
*
* Helper class for different modes in navigator
* Base class for different modes in navigator
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_MODE_H
@ -65,11 +66,18 @@ public:
*/
virtual ~NavigatorMode();
bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called one time when mode become active, poos_sp_triplet must be initialized here
*/
virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* This function is called while the mode is active
*
@ -80,6 +88,8 @@ public:
protected:
Navigator *_navigator;
private:
bool _first_run;
};

64
src/modules/navigator/rtl.cpp
View File

@ -75,53 +75,49 @@ RTL::~RTL()
void
RTL::on_inactive()
{
_first_run = true;
/* reset RTL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_rtl_state = RTL_STATE_NONE;
}
}
void
RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item(pos_sp_triplet);
}
bool
RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
bool updated = false;
if (_first_run) {
_first_run = false;
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item(pos_sp_triplet);
updated = true;
} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
return true;
}
return updated;
return false;
}
void

20
src/modules/navigator/rtl.h
View File

@ -57,29 +57,15 @@ class Navigator;
class RTL : public MissionBlock
{
public:
/**
* Constructor
*/
RTL(Navigator *navigator, const char *name);
/**
* Destructor
*/
~RTL();
/**
* This function is called while the mode is inactive
*/
void on_inactive();
virtual void on_inactive();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet that needs to be set
* @return true if updated
*/
bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet);
virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
private:
/**