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

Merge pull request #1119 from PX4/navigator_rewrite_drton 

Navigator rewrite - DrTon
This commit is contained in:
Lorenz Meier 2014-07-07 14:30:38 +02:00
parent 77ca6fa8a3 06f08ad04d
commit be73ad0bdb
14 changed files with 372 additions and 320 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/commander/commander.cpp
View File

@ -1747,6 +1747,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
}
print_reject_mode(status, "AUTO_MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
}
// fallback to POSCTL

2
src/modules/commander/state_machine_helper.cpp
View File

@ -271,7 +271,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
@ -279,6 +278,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {

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

@ -36,6 +36,7 @@
* Helper class to loiter
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <string.h>
@ -51,28 +52,42 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include "loiter.h"
#include "navigator.h"
Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
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()
{
/* set current mission item to loiter */
set_loiter_item(&_mission_item);
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();
}
void
Loiter::on_active()
{
}

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

@ -50,25 +50,15 @@
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 bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_activation();
virtual void on_active();
};
#endif

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

@ -54,25 +54,28 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "navigator.h"
#include "mission.h"
#include "navigator.h"
Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_param_takeoff_alt(this, "TAKEOFF_ALT"),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE)
_mission_type(MISSION_TYPE_NONE),
_need_takeoff(true),
_takeoff(false)
{
/* load initial params */
updateParams();
/* set initial mission items */
on_inactive();
}
Mission::~Mission()
@ -82,8 +85,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);
@ -96,13 +97,21 @@ Mission::on_inactive()
if (offboard_updated) {
update_offboard_mission();
}
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
}
bool
Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
void
Mission::on_activation()
{
bool updated = false;
set_mission_items();
}
void
Mission::on_active()
{
/* check if anything has changed */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@ -117,20 +126,21 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated || _first_run) {
set_mission_items(pos_sp_triplet);
updated = true;
_first_run = false;
if (onboard_updated || offboard_updated) {
set_mission_items();
}
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission();
set_mission_items(pos_sp_triplet);
updated = true;
}
set_mission_items();
return updated;
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
_navigator->set_can_loiter_at_sp(true);
}
}
}
void
@ -151,6 +161,7 @@ Mission::update_onboard_mission()
}
/* otherwise, just leave it */
}
} else {
_onboard_mission.count = 0;
_onboard_mission.current_index = 0;
@ -167,10 +178,12 @@ Mission::update_offboard_mission()
if (_offboard_mission.current_index >= 0
&& _offboard_mission.current_index < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_offboard_mission_index < 0) {
_current_offboard_mission_index = 0;
@ -184,6 +197,7 @@ Mission::update_offboard_mission()
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
@ -192,11 +206,13 @@ Mission::update_offboard_mission()
(size_t)_offboard_mission.count,
_navigator->get_geofence(),
_navigator->get_home_position()->alt);
} else {
_offboard_mission.count = 0;
_offboard_mission.current_index = 0;
_current_offboard_mission_index = 0;
}
report_current_offboard_mission_item();
}
@ -204,46 +220,55 @@ Mission::update_offboard_mission()
void
Mission::advance_mission()
{
switch (_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
if (_takeoff) {
_takeoff = false;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
} else {
switch (_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}
}
void
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
Mission::set_mission_items()
{
set_previous_pos_setpoint(pos_sp_triplet);
/* make sure param is up to date */
updateParams();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* try setting onboard mission item */
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: onboard mission running");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
}
_mission_type = MISSION_TYPE_ONBOARD;
_navigator->set_can_loiter_at_sp(false);
/* try setting offboard mission item */
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
} else if (read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: offboard mission running");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
_navigator->set_can_loiter_at_sp(false);
} else {
/* no mission available, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: mission finished");
@ -252,125 +277,152 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: no mission available");
}
_mission_type = MISSION_TYPE_NONE;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
set_loiter_item(pos_sp_triplet);
/* set loiter mission item */
set_loiter_item(&_mission_item);
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
reset_mission_item_reached();
report_mission_finished();
_navigator->set_position_setpoint_triplet_updated();
return;
}
}
bool
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
/* make sure param is up to date */
updateParams();
if (_param_onboard_enabled.get() > 0 &&
_current_onboard_mission_index >= 0&&
_current_onboard_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
&new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
/* do takeoff on first waypoint for rotary wing vehicles */
if (_navigator->get_vstatus()->is_rotary_wing) {
/* force takeoff if landed (additional protection) */
if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
reset_mission_item_reached();
/* TODO: report this somehow */
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
return true;
/* new current mission item set, check if we need takeoff */
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_takeoff = true;
_need_takeoff = false;
}
}
return false;
}
bool
Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
if (_current_offboard_mission_index >= 0 &&
_current_offboard_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
if (_takeoff) {
/* do takeoff before going to setpoint */
/* set mission item as next position setpoint */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
/* calculate takeoff altitude */
float takeoff_alt = _mission_item.altitude;
if (_mission_item.altitude_is_relative) {
takeoff_alt += _navigator->get_home_position()->alt;
}
/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
reset_mission_item_reached();
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt);
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
} else {
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
report_current_offboard_mission_item();
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
return true;
}
}
return false;
}
// TODO: report onboard mission item somehow
void
Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
int next_temp_mission_index = _onboard_mission.current_index + 1;
/* try to read next mission item */
struct mission_item_s mission_item_next;
/* try if there is a next onboard mission */
if (_onboard_mission.current_index >= 0 &&
next_temp_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
/* give up */
next_pos_sp->valid = false;
return;
}
void
Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
/* try if there is a next offboard mission */
int next_temp_mission_index = _offboard_mission.current_index + 1;
warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
if (_offboard_mission.current_index >= 0 &&
next_temp_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
}
/* give up */
next_pos_sp->valid = false;
return;
_navigator->set_position_setpoint_triplet_updated();
}
bool
Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item)
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
{
/* select onboard/offboard mission */
int *mission_index_ptr;
struct mission_s *mission;
dm_item_t dm_item;
int mission_index_next;
if (onboard) {
/* onboard mission */
mission_index_next = _current_onboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
mission = &_onboard_mission;
dm_item = DM_KEY_WAYPOINTS_ONBOARD;
} else {
/* offboard mission */
mission_index_next = _current_offboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
mission = &_offboard_mission;
if (_offboard_mission.dataman_id == 0) {
dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
}
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
return false;
}
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
for (int i=0; i<10; i++) {
for (int i = 0; i < 10; i++) {
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item to temp storage first to not overwrite current mission item if data damaged */
struct mission_item_s mission_item_tmp;
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR waypoint could not be read");
@ -378,18 +430,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
}
/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {
/* do DO_JUMP as many times as requested */
if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {
/* only raise the repeat count if this is for the current mission item
* but not for the next mission item */
if (is_current) {
(new_mission_item->do_jump_current_count)++;
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
new_mission_item, len) != len) {
if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
@ -399,16 +450,18 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index;
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index)++;
(*mission_index_ptr)++;
}
} else {
/* if it's not a DO_JUMP, then we were successful */
memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
return true;
}
}

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

@ -65,25 +65,15 @@ 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 bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_activation();
virtual void on_active();
private:
/**
@ -104,36 +94,13 @@ private:
/**
* Set new mission items
*/
void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
void set_mission_items();
/**
* Try to set the current position setpoint from an onboard mission item
* @return true if mission item successfully set
*/
bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the current position setpoint from an offboard mission item
* @return true if mission item successfully set
*/
bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the next position setpoint from an onboard mission item
*/
void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Try to set the next position setpoint from an offboard mission item
*/
void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Read a mission item from the dataman and watch out for DO_JUMPS
* Read current or next mission item from the dataman and watch out for DO_JUMPS
* @return true if successful
*/
bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item);
bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item);
/**
* Report that a mission item has been reached
@ -156,12 +123,15 @@ private:
void publish_mission_result();
control::BlockParamFloat _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;
int _current_onboard_mission_index;
int _current_offboard_mission_index;
bool _need_takeoff;
bool _takeoff;
orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result;

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

@ -47,6 +47,7 @@
#include <systemlib/err.h>
#include <geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <uORB/uORB.h>
@ -59,8 +60,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_mission_item({0}),
_mission_item_valid(false)
_mission_item({0})
{
}
@ -71,6 +71,10 @@ MissionBlock::~MissionBlock()
bool
MissionBlock::is_mission_item_reached()
{
if (_mission_item.nav_cmd == NAV_CMD_IDLE) {
return false;
}
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return _navigator->get_vstatus()->condition_landed;
}
@ -84,7 +88,6 @@ MissionBlock::is_mission_item_reached()
hrt_abstime now = hrt_absolute_time();
if (!_waypoint_position_reached) {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
@ -201,44 +204,48 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
}
void
MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
MissionBlock::set_previous_pos_setpoint()
{
/* reuse current setpoint as previous setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (pos_sp_triplet->current.valid) {
memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
}
}
bool
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
void
MissionBlock::set_loiter_item(struct mission_item_s *item)
{
/* 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 */
item->nav_cmd = NAV_CMD_IDLE;
_navigator->set_can_loiter_at_sp(true);
} else {
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
/* use current position setpoint */
item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt;
} else {
/* use current position */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
}
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->pitch_min = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON)
|| 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;
}

14
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
@ -85,22 +86,17 @@ public:
/**
* Set previous position setpoint to current setpoint
*/
void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
void set_previous_pos_setpoint();
/**
* 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);
void set_loiter_item(struct mission_item_s *item);
mission_item_s _mission_item;
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;
mission_item_s _mission_item;
bool _mission_item_valid;
};
#endif

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

@ -103,6 +103,7 @@ public:
* Setters
*/
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
/**
* Getters
@ -110,6 +111,7 @@ public:
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence& get_geofence() { return _geofence; }
@ -162,7 +164,7 @@ private:
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */

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

@ -121,7 +121,7 @@ Navigator::Navigator() :
_mission(this, "MIS"),
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_update_triplet(false),
_pos_sp_triplet_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD")
{
@ -371,24 +371,21 @@ 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();
}
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
/* if nothing is running, set position setpoint triplet invalid */
if (_navigation_mode == nullptr) {
// TODO publish empty sp only once
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
_pos_sp_triplet_updated = true;
}
if (_update_triplet) {
if (_pos_sp_triplet_updated) {
publish_position_setpoint_triplet();
_update_triplet = false;
_pos_sp_triplet_updated = false;
}
perf_end(_loop_perf);

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

@ -33,12 +33,14 @@
/**
* @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"
#include "navigator.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
SuperBlock(NULL, name),
@ -56,15 +58,38 @@ NavigatorMode::~NavigatorMode()
}
void
NavigatorMode::on_inactive()
{
_first_run = true;
NavigatorMode::run(bool active) {
if (active) {
if (_first_run) {
/* first run */
_first_run = false;
on_activation();
} else {
/* periodic updates when active */
on_active();
}
} else {
/* periodic updates when inactive */
_first_run = true;
on_inactive();
}
}
bool
NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
void
NavigatorMode::on_inactive()
{
}
void
NavigatorMode::on_activation()
{
/* invalidate position setpoint by default */
_navigator->get_position_setpoint_triplet()->current.valid = false;
}
void
NavigatorMode::on_active()
{
pos_sp_triplet->current.valid = false;
_first_run = false;
return false;
}

19
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,21 +66,27 @@ public:
*/
virtual ~NavigatorMode();
void run(bool active);
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet to set
* @return true if position setpoint triplet has been changed
* This function is called one time when mode become active, poos_sp_triplet must be initialized here
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_activation();
/**
* This function is called while the mode is active
*/
virtual void on_active();
protected:
Navigator *_navigator;
private:
bool _first_run;
};

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

@ -75,62 +75,57 @@ 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;
}
}
bool
RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
void
RTL::on_activation()
{
bool updated = 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 (_first_run) {
_first_run = false;
/* 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;
/* 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;
}
/* 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()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
}
return updated;
set_rtl_item();
}
void
RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
RTL::on_active()
{
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item();
}
}
void
RTL::set_rtl_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* make sure we have the latest params */
updateParams();
set_previous_pos_setpoint(pos_sp_triplet);
set_previous_pos_setpoint();
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
@ -277,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
break;
}
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
void

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

@ -57,35 +57,21 @@ 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();
virtual void on_active();
private:
/**
* Set the RTL item
*/
void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
void set_rtl_item();
/**
* Move to next RTL item