zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Navigator: More improvements, loiter at the end works

sbg
Julian Oes 11 years ago
parent
8535d12e60
commit
42aefe838c
1 changed files with 123 additions and 181 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 300
      src/modules/navigator/navigator_main.cpp

300
src/modules/navigator/navigator_main.cpp
Unescape View File

@ -1,9 +1,9 @@ @@ -1,9 +1,9 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Authors: Lorenz Meier
* Jean Cyr
* Julian Oes
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -69,6 +69,11 @@ @@ -69,6 +69,11 @@
#include <mathlib/mathlib.h>
#include <dataman/dataman.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/**
* navigator app start / stop handling function
@ -208,9 +213,17 @@ private: @@ -208,9 +213,17 @@ private:
bool fence_valid(const struct fence_s &fence);
void change_current_mission_item(int new_mission_item_index);
int add_mission_item(unsigned mission_item, struct mission_item_s *new_mission_item);
void add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item);
bool mission_item_reached();
void advance_current_mission_item();
void restart_mission();
void reset_mission_item_reached();
bool check_mission_item_reached();
};
namespace navigator
@ -317,7 +330,7 @@ Navigator::mission_update() @@ -317,7 +330,7 @@ Navigator::mission_update()
irqrestore(flags);
/* start new mission at beginning */
change_current_mission_item(0);
restart_mission();
} else {
warnx("ERROR: too many waypoints, not supported");
}
@ -448,101 +461,18 @@ Navigator::task_main() @@ -448,101 +461,18 @@ Navigator::task_main()
}
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
// Current waypoint
/* Current waypoint */
math::Vector2f next_wp(_mission_item_triplet.current.lat / 1e7f, _mission_item_triplet.current.lon / 1e7f);
// Global position
/* Global position */
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
/* AUTONOMOUS FLIGHT */
/* Autonomous flight */
if (1 /* autonomous flight */) {
/* proceed to next waypoint if we reach it */
if (mission_item_reached()) {
change_current_mission_item(_current_mission_item_index + 1);
}
/* execute navigation once we have a setpoint */
if (_mission_item_count > 0) {
// Next waypoint
math::Vector2f prev_wp;
if (_mission_item_triplet.previous_valid) {
prev_wp.setX(_mission_item_triplet.previous.lat / 1e7f);
prev_wp.setY(_mission_item_triplet.previous.lon / 1e7f);
} else {
/*
* No valid next waypoint, go for heading hold.
* This is automatically handled by the L1 library.
*/
prev_wp.setX(_mission_item_triplet.current.lat / 1e7f);
prev_wp.setY(_mission_item_triplet.current.lon / 1e7f);
}
/******** MAIN NAVIGATION STATE MACHINE ********/
// XXX to be put in its own class
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
/* waypoint is a plain navigation waypoint */
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
/* waypoint is a loiter waypoint */
}
// XXX at this point we always want no loiter hold if a
// mission is active
_loiter_hold = false;
} else {
if (!_loiter_hold) {
_loiter_hold_lat = _global_pos.lat / 1e7f;
_loiter_hold_lon = _global_pos.lon / 1e7f;
_loiter_hold_alt = _global_pos.alt;
_loiter_hold = true;
}
//_parameters.loiter_hold_radius
if (check_mission_item_reached()) {
advance_current_mission_item();
}
} else if (0/* seatbelt mode enabled */) {
/** SEATBELT FLIGHT **/
continue;
} else {
/** MANUAL FLIGHT **/
/* no flight mode applies, do not publish an attitude setpoint */
continue;
}
/******** MAIN NAVIGATION STATE MACHINE ********/
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
// XXX define launch position and return
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
// XXX flared descent on final approach
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
/* apply minimum pitch if altitude has not yet been reached */
// if (_global_pos.alt < _mission_item_triplet.current.altitude) {
// _att_sp.pitch_body = math::max(_att_sp.pitch_body, _mission_item_triplet.current.param1);
// }
}
/* lazily publish the setpoint only once available */
@ -554,7 +484,6 @@ Navigator::task_main() @@ -554,7 +484,6 @@ Navigator::task_main()
/* advertise and publish */
_triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet);
}
}
perf_end(_loop_perf);
@ -697,39 +626,70 @@ Navigator::fence_point(int argc, char *argv[]) @@ -697,39 +626,70 @@ Navigator::fence_point(int argc, char *argv[])
errx(1, "can't store fence point");
}
int
Navigator::add_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) {
/* Check if there is a further mission as the new next item */
while (mission_item_index < _mission_item_count) {
if (1 /* TODO: check for correct frame */) {
warnx("copying item number %d", mission_item_index);
memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s));
return OK;
}
mission_item_index++;
}
return ERROR;
}
void
Navigator::change_current_mission_item(int new_mission_item_index)
Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item)
{
/* no change */
if (new_mission_item_index == _current_mission_item_index) {
return;
}
/* or maybe there are no more mission items */
if (new_mission_item_index >= _mission_item_count) {
/* if no existing mission item exists, take curent location */
if (existing_mission_item == nullptr) {
new_mission_item->lat = (double)_global_pos.lat / 1e7;
new_mission_item->lon = (double)_global_pos.lon / 1e7;
new_mission_item->altitude = _global_pos.alt;
new_mission_item->yaw = _global_pos.yaw;
new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
new_mission_item->loiter_direction = 1;
new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number
new_mission_item->radius = 10.0f; // TODO: get rid of magic number
new_mission_item->autocontinue = false;
/* just keep the last mission item and set it to loiter */
_mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false;
} else {
switch (_mission_item_triplet.current.nav_cmd) {
switch (existing_mission_item->nav_cmd) {
/* if the last mission is not a loiter item, set it to one */
case NAV_CMD_WAYPOINT:
case NAV_CMD_RETURN_TO_LAUNCH:
case NAV_CMD_TAKEOFF:
_mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item_triplet.current.loiter_radius = 100.0f;
/* copy current mission to next item */
memcpy(new_mission_item, existing_mission_item, sizeof(mission_item_s));
/* and set it to a loiter item */
new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
/* also adapt the loiter_radius */
new_mission_item->loiter_radius = 100.0f;
//_mission_item_triplet.current.loiter_radius = _nav_caps.turn_distance; // TODO: publish capabilities somewhere
_mission_item_triplet.current.loiter_direction = 1;
new_mission_item->loiter_direction = 1;
break;
/* if the last mission item was to loiter, continue unlimited */
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_TIME_LIMIT:
_mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
/* copy current mission to next item */
memcpy(new_mission_item, existing_mission_item, sizeof(mission_item_s));
/* and set it to loiter */
new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
break;
/* if already in loiter, don't change anything */
case NAV_CMD_LOITER_UNLIMITED:
@ -743,99 +703,84 @@ Navigator::change_current_mission_item(int new_mission_item_index) @@ -743,99 +703,84 @@ Navigator::change_current_mission_item(int new_mission_item_index)
break;
}
}
else {
/* accept new mission item */
_current_mission_item_index = new_mission_item_index;
/* reset all states */
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
_time_first_inside_orbit = 0;
}
/* TODO: extend this to different frames, global for now */
void
Navigator::advance_current_mission_item()
{
/* if there is one more mission available we can just advance by one, otherwise return */
if (_mission_item_triplet.next_valid) {
_mission_item_triplet.current_valid = false;
reset_mission_item_reached();
if (_mission_item_count > 0 && new_mission_item_index < _mission_item_count) {
_mission_item_triplet.current_valid = true;
memcpy(&_mission_item_triplet.current, &_mission_item[new_mission_item_index], sizeof(mission_item_s));
}
/* copy current mission to previous item */
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
int previous_setpoint_index = -1;
_mission_item_triplet.previous_valid = false;
/* copy the next to current */
memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s));
_mission_item_triplet.current_valid = _mission_item_triplet.next_valid;
if (new_mission_item_index > 0) {
previous_setpoint_index = new_mission_item_index - 1;
}
_current_mission_item_index++;
while (previous_setpoint_index >= 0) {
if ((_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT ||
_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS ||
_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME ||
_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
/* maybe there are no more mission item, in this case add a loiter mission item */
if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) {
_mission_item_triplet.previous_valid = true;
memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s));
break;
}
previous_setpoint_index--;
add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next);
_mission_item_triplet.next_valid = true;
}
}
}
/*
* Check if next WP (in mission, not in execution order)
* is available and identify correct index
*/
int next_setpoint_index = -1;
_mission_item_triplet.next_valid = false;
/* next waypoint */
if (_mission_item_count > 1) {
next_setpoint_index = new_mission_item_index + 1;
}
void
Navigator::restart_mission()
{
reset_mission_item_reached();
while (next_setpoint_index < _mission_item_count - 1) {
_current_mission_item_index = 0;
if ((_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT ||
_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS ||
_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME ||
_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
_mission_item_triplet.previous_valid = false;
_mission_item_triplet.next_valid = true;
memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s));
break;
}
/* add a new current mission item */
if (add_mission_item(_current_mission_item_index, &_mission_item_triplet.current) != OK) {
next_setpoint_index++;
add_last_mission_item(nullptr, &_mission_item_triplet.current);
} else {
/* if current succeeds, we can even add a next item */
if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) {
add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next);
}
_mission_item_triplet.next_valid = true;
}
_mission_item_triplet.current_valid = true;
}
/* lazily publish the setpoint only once available */
if (_triplet_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet);
} else {
/* advertise and publish */
_triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet);
}
}
void
Navigator::reset_mission_item_reached()
{
/* reset all states */
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
_time_first_inside_orbit = 0;
}
bool
Navigator::mission_item_reached()
Navigator::check_mission_item_reached()
{
uint64_t now = hrt_absolute_time();
float orbit;
if (_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_WAYPOINT) {
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
orbit = _mission_item_triplet.current.radius;
} else if (_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS ||
_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME ||
_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM) {
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
orbit = _mission_item_triplet.current.loiter_radius;
} else {
@ -847,9 +792,6 @@ Navigator::mission_item_reached() @@ -847,9 +792,6 @@ Navigator::mission_item_reached()
/* keep vertical orbit */
float vertical_switch_distance = orbit;
/* Take the larger turn distance - orbit or turn_distance */
if (orbit < _nav_caps.turn_distance)
orbit = _nav_caps.turn_distance;
// TODO add frame
// int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;

Write Preview
Loading…
Cancel
Save