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

navigator: API changes, reparing to move manual modes to navigator, WIP

This commit is contained in:
Anton Babushkin 2014-06-29 14:09:22 +02:00
parent 12be974bd6
commit 0bf9c2a9b2
10 changed files with 87 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
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,14 +52,13 @@
#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()
@ -71,16 +71,23 @@ Loiter::on_inactive()
}
void
Loiter::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
Loiter::on_activation()
{
set_loiter_item(pos_sp_triplet);
/* 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();
}
bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
void
Loiter::on_active()
{
return false;
}

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

@ -56,9 +56,9 @@ public:
virtual void on_inactive();
virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_activation();
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_active();
};
#endif

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

@ -54,8 +54,8 @@
#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),
@ -97,16 +97,14 @@ Mission::on_inactive()
}
void
Mission::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
Mission::on_activation()
{
set_mission_items(pos_sp_triplet);
set_mission_items();
}
bool
Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
void
Mission::on_active()
{
bool updated = false;
/* check if anything has changed */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@ -122,18 +120,18 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
/* reset mission items if needed */
if (onboard_updated || offboard_updated) {
set_mission_items(pos_sp_triplet);
updated = true;
set_mission_items();
_navigator->set_position_setpoint_triplet_updated();
}
/* 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;
_navigator->set_position_setpoint_triplet_updated();
}
}
void
@ -223,8 +221,10 @@ Mission::advance_mission()
}
void
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
Mission::set_mission_items()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
set_previous_pos_setpoint(pos_sp_triplet);
/* try setting onboard mission item */
@ -235,7 +235,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: onboard mission running");
}
_mission_type = MISSION_TYPE_ONBOARD;
_navigator->set_can_loiter_at_sp(false);
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
/* try setting offboard mission item */
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@ -245,7 +245,8 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: offboard mission running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
_navigator->set_can_loiter_at_sp(false);
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
} else {
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
@ -255,12 +256,15 @@ 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_item(&_mission_item);
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();
}
@ -279,7 +283,6 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
&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();
@ -306,7 +309,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
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();
@ -330,7 +332,6 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
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;
}
}
@ -358,7 +359,6 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
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;
}
}

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

@ -71,9 +71,9 @@ public:
virtual void on_inactive();
virtual void on_activation(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_activation();
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_active();
private:
/**
@ -94,7 +94,7 @@ 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

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

@ -58,8 +58,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})
{
}
@ -70,6 +69,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;
}
@ -83,7 +86,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;
@ -209,43 +211,38 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_
}
void
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
MissionBlock::set_loiter_item(struct mission_item_s *item)
{
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(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "landed, switch to IDLE");
item->nav_cmd = NAV_CMD_IDLE;
} else {
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
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 */
_mission_item.lat = pos_sp_triplet->current.lat;
_mission_item.lon = pos_sp_triplet->current.lon;
_mission_item.altitude = pos_sp_triplet->current.alt;
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 */
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
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");
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;
}
}

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

@ -91,11 +91,9 @@ protected:
/**
* Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
*/
// 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);
void set_loiter_item(struct mission_item_s *item);
mission_item_s _mission_item;
bool _mission_item_valid;
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;

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 */

11
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,20 +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++) {
_update_triplet = _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], &_pos_sp_triplet);
_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);

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

@ -40,6 +40,7 @@
*/
#include "navigator_mode.h"
#include "navigator.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
SuperBlock(NULL, name),
@ -56,18 +57,17 @@ NavigatorMode::~NavigatorMode()
{
}
bool
NavigatorMode::run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet) {
void
NavigatorMode::run(bool active) {
if (active) {
if (_first_run) {
/* first run */
_first_run = false;
on_activation(pos_sp_triplet);
return true;
on_activation();
} else {
/* periodic updates when active */
on_active(pos_sp_triplet);
on_active();
}
} else {
@ -83,14 +83,13 @@ NavigatorMode::on_inactive()
}
void
NavigatorMode::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
NavigatorMode::on_activation()
{
/* invalidate position setpoint by default */
pos_sp_triplet->current.valid = false;
_navigator->get_position_setpoint_triplet()->current.valid = false;
}
bool
NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
void
NavigatorMode::on_active()
{
return false;
}

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

@ -66,7 +66,7 @@ public:
*/
virtual ~NavigatorMode();
bool run(bool active, struct position_setpoint_triplet_s *pos_sp_triplet);
void run(bool active);
/**
* This function is called while the mode is inactive
@ -76,15 +76,12 @@ public:
/**
* 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);
virtual void on_activation();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet to set
* @return true if position setpoint triplet has been changed
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_active();
protected:
Navigator *_navigator;