diff options
author | Julian Oes <julian@oes.ch> | 2014-06-03 16:01:28 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-06-03 16:01:28 +0200 |
commit | 854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7 (patch) | |
tree | 947f86c23d10ee717a418badb662ed09694687dd /src/modules/navigator | |
parent | 5f91fe7d15e382b8903cb01c30e28c857f5cfdbe (diff) | |
download | px4-firmware-854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7.tar.gz px4-firmware-854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7.tar.bz2 px4-firmware-854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7.zip |
navigator: mission class added (WIP)
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/mission.cpp | 438 | ||||
-rw-r--r-- | src/modules/navigator/mission.h | 164 | ||||
-rw-r--r-- | src/modules/navigator/module.mk | 1 | ||||
-rw-r--r-- | src/modules/navigator/navigator.h | 205 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 338 | ||||
-rw-r--r-- | src/modules/navigator/rtl.cpp | 31 | ||||
-rw-r--r-- | src/modules/navigator/rtl.h | 21 |
7 files changed, 730 insertions, 468 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index adbb0cfa2..f3a86666f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -38,30 +38,42 @@ * @author Julian Oes <julian@oes.ch> */ +#include <sys/types.h> #include <string.h> #include <stdlib.h> +#include <unistd.h> + +#include <drivers/drv_hrt.h> #include <dataman/dataman.h> #include <systemlib/err.h> +#include <geo/geo.h> #include <uORB/uORB.h> #include <uORB/topics/mission_result.h> +#include "navigator.h" #include "mission.h" -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), +Mission::Mission(Navigator *navigator) : + SuperBlock(NULL, "MIS"), + _navigator(navigator), + _first_run(true), + _param_onboard_enabled(this, "ONBOARD_EN"), + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), + _onboard_mission({0}), + _offboard_mission({0}), + _mission_item({0}), _mission_result_pub(-1), - _mission_result({}) + _mission_result({0}), + _mission_type(MISSION_TYPE_NONE) { + /* load initial params */ + updateParams(); + /* set initial mission items */ + reset(); } Mission::~Mission() @@ -69,134 +81,335 @@ Mission::~Mission() } void -Mission::set_offboard_dataman_id(const int new_id) +Mission::reset() { - _offboard_dataman_id = new_id; + _first_run = true; } -void -Mission::set_offboard_mission_count(int new_count) +bool +Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - _offboard_mission_item_count = new_count; + bool updated = false; + /* check if anything has changed, and reset mission items if needed */ + if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) { + set_mission_items(pos_sp_triplet); + updated = true; + _first_run = false; + } + + if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + advance_mission(); + set_mission_items(pos_sp_triplet); + updated = true; + } + return updated; } -void -Mission::set_onboard_mission_count(int new_count) +bool +Mission::is_mission_item_reached() { - _onboard_mission_item_count = new_count; + /* TODO: count turns */ +#if 0 + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { + + return false; + } +#endif + + hrt_abstime now = hrt_absolute_time(); + + if (!_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _navigator->get_home_position()->alt + : _mission_item.altitude; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } + } + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + /* TODO: removed takeoff, why? */ + if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + + /* check yaw if defined only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); + + if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ + _waypoint_yaw_reached = true; + } + + } else { + _waypoint_yaw_reached = true; + } + } + + /* check if the current waypoint was reached */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { + + if (_time_first_inside_orbit == 0) { + _time_first_inside_orbit = now; + + // if (_mission_item.time_inside > 0.01f) { + // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + // (double)_mission_item.time_inside); + // } + } + + /* check if the MAV was long enough inside the waypoint orbit */ + if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + return true; + } + } + return false; } void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; +Mission::reset_mission_item_reached() { + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; } -bool -Mission::command_current_offboard_mission_index(const int new_index) +void +Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { - if (new_index >= 0) { - _current_offboard_mission_index = (unsigned)new_index; - } else { + sp->valid = true; - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_POSITION; } - report_current_offboard_mission_item(); } bool -Mission::command_current_onboard_mission_index(int new_index) +Mission::is_onboard_mission_updated() { - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { + bool updated; + orb_check(_onboard_mission_sub, &updated); - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } + if (!updated) { + return false; } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); + + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) { + _onboard_mission.count = 0; + _onboard_mission.current_index = 0; + } + return true; } bool -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +Mission::is_offboard_mission_updated() { - *onboard = false; - *index = -1; - - /* try onboard mission first */ - if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - return true; - } + bool updated; + orb_check(_offboard_mission_sub, &updated); + if (!updated) { + return false; } - /* otherwise fallback to offboard */ - if (_current_offboard_mission_index < _offboard_mission_item_count) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) { + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; - if (_offboard_dataman_id == 0) { + + if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, + (size_t)_offboard_mission.count, + _navigator->get_geofence()); + } else { + _offboard_mission.count = 0; + _offboard_mission.current_index = 0; + } + return true; +} - return true; - } + +void +Mission::advance_mission() +{ + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _onboard_mission.current_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _offboard_mission.current_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } +} + +void +Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + warnx("set mission items"); + + set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; + if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting onboard mission item */ + _mission_type = MISSION_TYPE_ONBOARD; + } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting offboard mission item */ + _mission_type = MISSION_TYPE_OFFBOARD; + } else { + _mission_type = MISSION_TYPE_NONE; + } +} + +void +Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, + struct position_setpoint_s *previous_pos_sp) +{ + /* reuse current setpoint as previous setpoint */ + if (current_pos_sp->valid) { + memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s)); + } +} + +bool +Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +{ + if (_param_onboard_enabled.get() > 0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index, + &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; + + /* TODO: report this somehow */ + return true; + } + } return false; } bool -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - int next_temp_mission_index = _current_onboard_mission_index + 1; + if (_offboard_mission.current_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, true, &_offboard_mission.current_index, &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; - /* try onboard mission first */ - if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + report_current_offboard_mission_item(); return true; } } + return false; +} - /* then try offboard mission */ - dm_item_t dm_current; - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - next_temp_mission_index = _current_offboard_mission_index + 1; - if (next_temp_mission_index < _offboard_mission_item_count) { - if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { - return true; +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 if there is a next onboard mission */ + if (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; } } - /* both failed, bail out */ - return false; + /* 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; + if (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; + } + } + /* give up */ + next_pos_sp->valid = false; + return; } 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(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item) { /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i=0; i<10; i++) { @@ -211,25 +424,25 @@ 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 (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + return false; + } - /* 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)++; + /* 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)++; - /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the dataman */ + return false; } - /* 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; - } else { - return false; } + /* 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; } else { /* if it's not a DO_JUMP, then we were successful */ @@ -243,31 +456,11 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } void -Mission::move_to_next() -{ - report_mission_item_reached(); - - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void Mission::report_mission_item_reached() { - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + if (_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.mission_index_reached = _offboard_mission.current_index; } publish_mission_result(); } @@ -275,7 +468,7 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + _mission_result.index_current_mission = _offboard_mission.current_index; publish_mission_result(); } @@ -294,4 +487,3 @@ Mission::publish_mission_result() /* reset reached bool */ _mission_result.mission_reached = false; } - diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5e0e9702d..e86dd25bb 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -31,70 +31,172 @@ * ****************************************************************************/ /** - * @file navigator_mission.h + * @file mission.h * Helper class to access missions - * @author Julian Oes <julian@oes.ch> * - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> */ #ifndef NAVIGATOR_MISSION_H #define NAVIGATOR_MISSION_H -#include <uORB/topics/mission.h> -#include <uORB/topics/mission_result.h> +#include <drivers/drv_hrt.h> + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + #include <dataman/dataman.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/mission_result.h> + +#include "mission_feasibility_checker.h" + +class Navigator; -class __EXPORT Mission +class Mission : public control::SuperBlock { public: /** * Constructor */ - Mission(); + Mission(Navigator *navigator); /** * Destructor */ - ~Mission(); + virtual ~Mission(); - void set_offboard_dataman_id(const int new_id); - void set_offboard_mission_count(const int new_count); - void set_onboard_mission_count(const int new_count); - void set_onboard_mission_allowed(const bool allowed); + /** + * This function is called while the mode is inactive + */ + virtual void reset(); - bool command_current_offboard_mission_index(const int new_index); - bool command_current_onboard_mission_index(const int new_index); + /** + * This function is called while the mode is active + */ + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); - bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index); - bool get_next_mission_item(struct mission_item_s *mission_item); +protected: + /** + * Check if mission item has been reached + * @return true if successfully reached + */ + bool is_mission_item_reached(); + /** + * Reset all reached flags + */ + void reset_mission_item_reached(); - void move_to_next(); + /** + * Convert a mission item to a position setpoint + */ + void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp); + + class Navigator *_navigator; + + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; + + bool _first_run; private: - bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + /** + * Update onboard mission topic + * @return true if onboard mission has been updated + */ + bool is_onboard_mission_updated(); + + /** + * Update offboard mission topic + * @return true if offboard mission has been updated + */ + bool is_offboard_mission_updated(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * Set new mission items + */ + void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); + + /** + * Set previous position setpoint + */ + void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, + struct position_setpoint_s *previous_pos_sp); + + /** + * 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); - void report_mission_item_reached(); - void report_current_offboard_mission_item(); + /** + * 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); - void publish_mission_result(); + /** + * 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); - int _offboard_dataman_id; - int _current_offboard_mission_index; - int _current_onboard_mission_index; - int _offboard_mission_item_count; /** number of offboard mission items available */ - int _onboard_mission_item_count; /** number of onboard mission items available */ - bool _onboard_mission_allowed; + /** + * Read a 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); + + /** + * Report that a mission item has been reached + */ + void report_mission_item_reached(); + + /** + * Rport the current mission item + */ + void report_current_offboard_mission_item(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + + control::BlockParamFloat _param_onboard_enabled; + + int _onboard_mission_sub; + int _offboard_mission_sub; + + struct mission_s _onboard_mission; + struct mission_s _offboard_mission; + + struct mission_item_s _mission_item; + + orb_advert_t _mission_result_pub; + struct mission_result_s _mission_result; enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; + MISSION_TYPE_OFFBOARD + } _mission_type; + + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - mission_result_s _mission_result; /**< mission result for commander/mavlink */ }; #endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 8913d4d1c..2d07bc49c 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -41,7 +41,6 @@ SRCS = navigator_main.cpp \ navigator_params.c \ mission.cpp \ rtl.cpp \ - rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h new file mode 100644 index 000000000..1838fe32b --- /dev/null +++ b/src/modules/navigator/navigator.h @@ -0,0 +1,205 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator.h + * Helper class to access missions + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef NAVIGATOR_H +#define NAVIGATOR_H + +#include <systemlib/perf_counter.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/vehicle_global_position.h> + +#include "mission.h" +#include "rtl.h" +#include "geofence.h" + +class Navigator +{ +public: + /** + * Constructor + */ + Navigator(); + + /** + * Destructor, also kills the navigators task. + */ + ~Navigator(); + + /** + * Start the navigator task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the navigator status. + */ + void status(); + + /** + * Add point to geofence + */ + void add_fence_point(int argc, char *argv[]); + + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + + 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; } + Geofence& get_geofence() { return _geofence; } + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _navigator_task; /**< task handle for sensor task */ + + int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ + + int _global_pos_sub; /**< global position subscription */ + int _home_pos_sub; /**< home position subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _offboard_mission_sub; /**< notification of offboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ + + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + mission_item_s _mission_item; /**< current mission item */ + navigation_capabilities_s _nav_caps; /**< navigation capabilities */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + + bool _mission_item_valid; /**< flags if the current mission item is valid */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Geofence _geofence; /**< class that handles the geofence */ + bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ + + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ + + Mission _mission; /**< class that handles the missions */ + //Loiter _loiter; /**< class that handles loiter */ + RTL _rtl; /**< class that handles RTL */ + + bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ + bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ + uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ + + bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ + + struct { + float acceptance_radius; + float loiter_radius; + int onboard_mission_enabled; + float takeoff_alt; + } _parameters; /**< local copies of parameters */ + + struct { + param_t acceptance_radius; + param_t loiter_radius; + param_t onboard_mission_enabled; + param_t takeoff_alt; + } _parameter_handles; /**< handles for parameters */ + + /** + * Update our local parameter cache. + */ + void parameters_update(); + + /** + * Retrieve global position + */ + void global_position_update(); + + /** + * Retrieve home position + */ + void home_position_update(); + + /** + * Retreive navigation capabilities + */ + void navigation_capabilities_update(); + + /** + * Retrieve vehicle status + */ + void vehicle_status_update(); + + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main task. + */ + void task_main(); + + /** + * Translate mission item to a position setpoint. + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + + /** + * Publish a new position setpoint triplet for position controllers + */ + void publish_position_setpoint_triplet(); +}; +#endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f30cd9a94..b991ffc8c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -62,11 +62,8 @@ #include <arch/board/board.h> #include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/home_position.h> -#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> @@ -74,24 +71,13 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <geo/geo.h> #include <dataman/dataman.h> #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include "mission.h" -#include "rtl.h" -#include "mission_feasibility_checker.h" -#include "geofence.h" - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include "navigator.h" /** * navigator app start / stop handling function @@ -100,174 +86,10 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator -{ -public: - /** - * Constructor - */ - Navigator(); - - /** - * Destructor, also kills the navigators task. - */ - ~Navigator(); - - /** - * Start the navigator task. - * - * @return OK on success. - */ - int start(); - - /** - * Display the navigator status. - */ - void status(); - - /** - * Add point to geofence - */ - void add_fence_point(int argc, char *argv[]); - - /** - * Load fence from file - */ - void load_fence_from_file(const char *filename); - -private: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _navigator_task; /**< task handle for sensor task */ - - int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ - - int _global_pos_sub; /**< global position subscription */ - int _home_pos_sub; /**< home position subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ - int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _control_mode_sub; /**< vehicle control mode subscription */ - - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - - vehicle_status_s _vstatus; /**< vehicle status */ - vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - vehicle_global_position_s _global_pos; /**< global vehicle position */ - home_position_s _home_pos; /**< home position for RTL */ - mission_item_s _mission_item; /**< current mission item */ - navigation_capabilities_s _nav_caps; /**< navigation capabilities */ - position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - - bool _mission_item_valid; /**< flags if the current mission item is valid */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Geofence _geofence; /**< class that handles the geofence */ - bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - - bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ - - - Mission _mission; /**< class that handles the missions */ - - RTL _rtl; /**< class that handles RTL */ - - bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ - bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ - uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ - - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - - bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ - - struct { - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - } _parameters; /**< local copies of parameters */ - - struct { - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - } _parameter_handles; /**< handles for parameters */ - - /** - * Update our local parameter cache. - */ - void parameters_update(); - - /** - * Retrieve global position - */ - void global_position_update(); - - /** - * Retrieve home position - */ - void home_position_update(); - - /** - * Retreive navigation capabilities - */ - void navigation_capabilities_update(); - - /** - * Retrieve offboard mission. - */ - void offboard_mission_update(); - - /** - * Retrieve onboard mission. - */ - void onboard_mission_update(); - - /** - * Retrieve vehicle status - */ - void vehicle_status_update(); - - /** - * Retrieve vehicle control mode - */ - void vehicle_control_mode_update(); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main task. - */ - void task_main(); - - /** - * Translate mission item to a position setpoint. - */ - void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - - /** - * Publish a new position setpoint triplet for position controllers - */ - void publish_position_setpoint_triplet(); -}; namespace navigator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Navigator *g_navigator; } @@ -299,8 +121,9 @@ Navigator::Navigator() : _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission({}), - _rtl({}), + _mission(this), + //_loiter(&_global_pos, &_home_pos, &_vstatus), + _rtl(this), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -351,7 +174,7 @@ Navigator::parameters_update() param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - _mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); + //_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); _geofence.updateParams(); } @@ -366,8 +189,6 @@ void Navigator::home_position_update() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - - _rtl.set_home_position(&_home_pos); } void @@ -376,54 +197,6 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } - -void -Navigator::offboard_mission_update() -{ - struct mission_s offboard_mission; - - if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { - - /* Check mission feasibility, for now do not handle the return value, - * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - 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; - } - - missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence); - - _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - - _mission.set_offboard_mission_count(offboard_mission.count); - _mission.command_current_offboard_mission_index(offboard_mission.current_index); - - } else { - _mission.set_offboard_mission_count(0); - _mission.command_current_offboard_mission_index(0); - } -} - -void -Navigator::onboard_mission_update() -{ - struct mission_s onboard_mission; - - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { - - _mission.set_onboard_mission_count(onboard_mission.count); - _mission.command_current_onboard_mission_index(onboard_mission.current_index); - - } else { - _mission.set_onboard_mission_count(0); - _mission.command_current_onboard_mission_index(0); - } -} - void Navigator::vehicle_status_update() { @@ -459,8 +232,7 @@ Navigator::task_main() /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file - * else clear geofence data in datamanager - */ + * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { @@ -474,9 +246,7 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* - * do subscriptions - */ + /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); @@ -493,8 +263,6 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(); - onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -503,7 +271,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -514,14 +282,10 @@ Navigator::task_main() fds[2].events = POLLIN; fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; - fds[4].fd = _offboard_mission_sub; + fds[4].fd = _vstatus_sub; fds[4].events = POLLIN; - fds[5].fd = _onboard_mission_sub; + fds[5].fd = _control_mode_sub; fds[5].events = POLLIN; - fds[6].fd = _vstatus_sub; - fds[6].events = POLLIN; - fds[7].fd = _control_mode_sub; - fds[7].events = POLLIN; while (!_task_should_exit) { @@ -547,12 +311,12 @@ Navigator::task_main() } /* vehicle control mode updated */ - if (fds[7].revents & POLLIN) { + if (fds[5].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[6].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_status_update(); } @@ -567,16 +331,6 @@ Navigator::task_main() navigation_capabilities_update(); } - /* offboard mission updated */ - if (fds[4].revents & POLLIN) { - offboard_mission_update(); - } - - /* onboard mission updated */ - if (fds[5].revents & POLLIN) { - onboard_mission_update(); - } - /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); @@ -600,6 +354,33 @@ Navigator::task_main() } } + /* Do stuff according to navigation state set by commander */ + switch (_vstatus.set_nav_state) { + case NAVIGATION_STATE_MANUAL: + case NAVIGATION_STATE_ACRO: + case NAVIGATION_STATE_ALTCTL: + case NAVIGATION_STATE_POSCTL: + break; + case NAVIGATION_STATE_AUTO_MISSION: + _update_triplet = _mission.update(&_pos_sp_triplet); + _rtl.reset(); + break; + case NAVIGATION_STATE_AUTO_LOITER: + //_loiter.update(); + break; + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + _mission.reset(); + _update_triplet = _rtl.update(&_pos_sp_triplet); + break; + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + default: + break; + + } + if (_update_triplet ) { publish_position_setpoint_triplet(); _update_triplet = false; @@ -880,47 +661,6 @@ Navigator::start_land() return true; } #endif -void -Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) -{ - sp->valid = true; - - if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); - - } else { - /* else use current position */ - sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); - } - - } else { - sp->lat = item->lat; - sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; - sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; - sp->loiter_direction = item->loiter_direction; - sp->pitch_min = item->pitch_min; - } - - if (item->nav_cmd == NAV_CMD_TAKEOFF) { - sp->type = SETPOINT_TYPE_TAKEOFF; - - } else if (item->nav_cmd == NAV_CMD_LAND) { - sp->type = SETPOINT_TYPE_LAND; - - } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - sp->type = SETPOINT_TYPE_LOITER; - - } else { - sp->type = SETPOINT_TYPE_NORMAL; - } -} #if 0 bool Navigator::check_mission_item_reached() diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b8ea06275..9d7886aa6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -51,26 +51,41 @@ #include "rtl.h" - -RTL::RTL() : - SuperBlock(NULL, "RTL"), +RTL::RTL(Navigator *navigator) : + Mission(navigator), _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), _home_position({}), + _loiter_radius(50), + _acceptance_radius(50), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY"), - _loiter_radius(50), - _acceptance_radius(50) + _param_land_delay(this, "LAND_DELAY") { /* load initial params */ updateParams(); + /* initial reset */ + reset(); } RTL::~RTL() { } +bool +RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated = false; + + return updated; +} + +void +RTL::reset() +{ + +} + void RTL::set_home_position(const home_position_s *new_home_position) { @@ -206,7 +221,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss } default: - return false; + return false; } return true; @@ -226,7 +241,7 @@ RTL::move_to_next() case RTL_STATE_CLIMB: _rtl_state = RTL_STATE_RETURN; break; - + case RTL_STATE_RETURN: _rtl_state = RTL_STATE_DESCEND; break; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c761837fc..8d1bfec59 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/*************************************************************************** * * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. * @@ -44,22 +44,32 @@ #include <controllib/block/BlockParam.hpp> #include <uORB/topics/mission.h> +#include <uORB/topics/mission.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_global_position.h> -class RTL : public control::SuperBlock +#include "mission.h" + +class Navigator; + +class RTL : public Mission { public: /** * Constructor */ - RTL(); + RTL(Navigator *navigator); /** * Destructor */ - ~RTL(); + virtual ~RTL(); + + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void reset(); + +private: void set_home_position(const home_position_s *home_position); bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); @@ -67,7 +77,6 @@ public: void move_to_next(); -private: int _mavlink_fd; enum RTLState { @@ -77,7 +86,7 @@ private: RTL_STATE_DESCEND, RTL_STATE_LAND, RTL_STATE_FINISHED, - } _rtl_state; + } _rtl_state; home_position_s _home_position; float _loiter_radius; |