/** Navigation function along a path
*/
static inline bool_t mission_nav_path(struct _mission_element *el)
{
if (el->element.mission_path.nb == 0) {
return FALSE; // nothing to do
}
if (el->element.mission_path.path_idx == 0) { //first wp of path
el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0];
if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
}
else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
struct EnuCoor_i *to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]);
//Check proximity and wait for t seconds in proximity circle if desired
if (nav_approaching_from(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(to_wp, el->duration)) {
el->element.mission_path.path_idx++;
}
} else { el->element.mission_path.path_idx++; }
}
//Route Between from-to
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(from_wp, to_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(from_wp->z), 0.);
} else { return FALSE; } //end of path
return TRUE;
}
/** Navigation function along a path
*/
static inline bool mission_nav_path(struct _mission_path *path)
{
if (path->nb == 0) {
return false; // nothing to do
}
if (path->nb == 1) {
// handle as a single waypoint
struct _mission_wp wp;
wp.wp.wp_f = path->path.path_f[0];
return mission_nav_wp(&wp);
}
if (path->path_idx == path->nb - 1) {
last_wp_f = path->path.path_f[path->path_idx]; // store last wp
return false; // end of path
}
// normal case
struct EnuCoor_f from_f = path->path.path_f[path->path_idx];
struct EnuCoor_f to_f = path->path.path_f[path->path_idx + 1];
nav_route_xy(from_f.x, from_f.y, to_f.x, to_f.y);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(to_f.z, 0.); // both altitude should be the same anyway
if (nav_approaching_xy(to_f.x, to_f.y, from_f.x, from_f.y, CARROT)) {
path->path_idx++; // go to next segment
}
return true;
}
int mission_run()
{
// current element
struct _mission_element *el = NULL;
if ((el = mission_get()) == NULL) {
// TODO do something special like a waiting circle before ending the mission ?
return false; // end of mission
}
bool el_running = false;
switch (el->type) {
case MissionWP:
el_running = mission_nav_wp(&(el->element.mission_wp));
break;
case MissionCircle:
el_running = mission_nav_circle(&(el->element.mission_circle));
break;
case MissionSegment:
el_running = mission_nav_segment(&(el->element.mission_segment));
break;
case MissionPath:
el_running = mission_nav_path(&(el->element.mission_path));
break;
case MissionCustom:
el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
break;
default:
// invalid type or pattern not yet handled
break;
}
// increment element_time
mission.element_time += dt_navigation;
// test if element is finished or element time is elapsed
if (!el_running || (el->duration > 0. && mission.element_time >= el->duration)) {
// reset timer
mission.element_time = 0.;
// go to next element
mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
}
return true;
}
int mission_run()
{
// current element
struct _mission_element *el = NULL;
if ((el = mission_get()) == NULL) {
mission.element_time = 0;
mission.current_idx = 0;
return FALSE; // end of mission
}
bool_t el_running = FALSE;
switch (el->type) {
case MissionWP:
el_running = mission_nav_wp(el);
break;
case MissionCircle:
el_running = mission_nav_circle(el);
break;
case MissionSegment:
el_running = mission_nav_segment(el);
break;
case MissionPath:
el_running = mission_nav_path(el);
break;
default:
// invalid type or pattern not yet handled
break;
}
// increment element_time
mission.element_time += dt_navigation;
if (!el_running) {
// reset timer
mission.element_time = 0.;
// go to next element
mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
}
return TRUE;
}
