diff options
Diffstat (limited to 'src/modules/navigator')
19 files changed, 2174 insertions, 1769 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index bc8dbca50..266215308 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { double lat = vehicle->lat / 1e7d; double lon = vehicle->lon / 1e7d; - float alt = vehicle->alt; + //float alt = vehicle->alt; return inside(lat, lon, vehicle->alt); } @@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude) } // skip vertex 0 (return point) - if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) && - (lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) / - (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) { + if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { c = !c; } @@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename) int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); + return OK; } diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp new file mode 100644 index 000000000..542483fb1 --- /dev/null +++ b/src/modules/navigator/loiter.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 loiter.cpp + * + * Helper class to loiter + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> + +#include <uORB/uORB.h> +#include <uORB/topics/position_setpoint_triplet.h> + +#include "loiter.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() +{ +} diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h new file mode 100644 index 000000000..65ff5c31e --- /dev/null +++ b/src/modules/navigator/loiter.h @@ -0,0 +1,74 @@ +/*************************************************************************** + * + * Copyright (c) 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 loiter.h + * + * Helper class to loiter + * + * @author Julian Oes <julian@oes.ch> + */ + +#ifndef NAVIGATOR_LOITER_H +#define NAVIGATOR_LOITER_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include "navigator_mode.h" +#include "mission_block.h" + +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); +}; + +#endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp new file mode 100644 index 000000000..72255103b --- /dev/null +++ b/src/modules/navigator/mission.cpp @@ -0,0 +1,461 @@ +/**************************************************************************** + * + * 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_mission.cpp + * + * Helper class to access missions + * + * @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 <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> + +#include "navigator.h" +#include "mission.h" + +Mission::Mission(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_onboard_enabled(this, "ONBOARD_EN"), + _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) +{ + /* load initial params */ + updateParams(); + /* set initial mission items */ + on_inactive(); + +} + +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); + if (onboard_updated) { + update_onboard_mission(); + } + + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } +} + +bool +Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated = false; + + /* check if anything has changed */ + bool onboard_updated; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } + + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + /* reset mission items if needed */ + if (onboard_updated || offboard_updated || _first_run) { + set_mission_items(pos_sp_triplet); + updated = true; + _first_run = false; + } + + /* 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; + } + + return updated; +} + +void +Mission::update_onboard_mission() +{ + if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { + /* accept the current index set by the onboard mission if it is within bounds */ + if (_onboard_mission.current_index >=0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_index; + } else { + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= (int)_onboard_mission.count) { + _current_onboard_mission_index = 0; + /* if not initialized, set it to 0 */ + } else if (_current_onboard_mission_index < 0) { + _current_onboard_mission_index = 0; + } + /* otherwise, just leave it */ + } + } else { + _onboard_mission.count = 0; + _onboard_mission.current_index = 0; + _current_onboard_mission_index = 0; + } +} + +void +Mission::update_offboard_mission() +{ + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { + + /* determine current index */ + 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; + } + /* otherwise, just leave it */ + } + + /* 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(_navigator->get_vstatus()->is_rotary_wing, dm_current, + (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(); +} + + +void +Mission::advance_mission() +{ + switch (_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::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_previous_pos_setpoint(pos_sp_triplet); + + /* try setting onboard mission item */ + if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_ONBOARD) { + 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)) { + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_OFFBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: offboard mission running"); + } + _mission_type = MISSION_TYPE_OFFBOARD; + _navigator->set_can_loiter_at_sp(false); + } else { + if (_mission_type != MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: mission finished"); + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#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); + reset_mission_item_reached(); + report_mission_finished(); + } +} + +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; + + reset_mission_item_reached(); + + /* TODO: report this somehow */ + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); + return true; + } + } + 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; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + 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(); + + report_current_offboard_mission_item(); + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); + return true; + } + } + return false; +} + +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 (_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; + } + } + + /* 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; + } + } + /* 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) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, 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"); + return false; + } + + /* 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) { + + /* do DO_JUMP as many times as requested */ + if (new_mission_item->do_jump_current_count < new_mission_item->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)++; + /* 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 */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); + 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 { + 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)++; + } + + } else { + /* if it's not a DO_JUMP, then we were successful */ + return true; + } + } + + /* we have given up, we don't want to cycle forever */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP is cycling, giving up"); + return false; +} + +void +Mission::report_mission_item_reached() +{ + if (_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } + publish_mission_result(); +} + +void +Mission::report_current_offboard_mission_item() +{ + _mission_result.index_current_mission = _current_offboard_mission_index; + publish_mission_result(); +} + +void +Mission::report_mission_finished() +{ + _mission_result.mission_finished = true; + publish_mission_result(); +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.mission_reached = false; + _mission_result.mission_finished = false; +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h new file mode 100644 index 000000000..6e4761946 --- /dev/null +++ b/src/modules/navigator/mission.h @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * 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 mission.h + * + * Navigator mode to access missions + * + * @author Julian Oes <julian@oes.ch> + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include <drivers/drv_hrt.h> + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <dataman/dataman.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> + +#include "navigator_mode.h" +#include "mission_block.h" +#include "mission_feasibility_checker.h" + +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); + +private: + /** + * Update onboard mission topic + */ + void update_onboard_mission(); + + /** + * Update offboard mission topic + */ + void update_offboard_mission(); + + /** + * 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); + + /** + * 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 + * @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(); + + /** + * Report that the mission is finished if one exists or that none exists + */ + void report_mission_finished(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + + control::BlockParamFloat _param_onboard_enabled; + + struct mission_s _onboard_mission; + struct mission_s _offboard_mission; + + int _current_onboard_mission_index; + int _current_offboard_mission_index; + + orb_advert_t _mission_result_pub; + struct mission_result_s _mission_result; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD + } _mission_type; + + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ +}; + +#endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp new file mode 100644 index 000000000..26a573544 --- /dev/null +++ b/src/modules/navigator/mission_block.cpp @@ -0,0 +1,244 @@ +/**************************************************************************** + * + * Copyright (c) 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 mission_block.cpp + * + * Helper class to use mission items + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <sys/types.h> +#include <string.h> +#include <stdlib.h> +#include <unistd.h> +#include <math.h> +#include <float.h> + +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> + +#include "navigator.h" +#include "mission_block.h" + + +MissionBlock::MissionBlock(Navigator *navigator, const char *name) : + NavigatorMode(navigator, name), + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), + _mission_item({0}), + _mission_item_valid(false) +{ +} + +MissionBlock::~MissionBlock() +{ +} + +bool +MissionBlock::is_mission_item_reached() +{ + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + return _navigator->get_vstatus()->condition_landed; + } + + /* TODO: count turns */ + if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + return false; + } + + 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 && _navigator->get_vstatus()->is_rotary_wing) { + /* require only altitude for takeoff for multicopter */ + if (_navigator->get_global_position()->alt > + altitude_amsl - _navigator->get_acceptance_radius()) { + _waypoint_position_reached = true; + } + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* for takeoff mission items use the parameter for the takeoff acceptance radius */ + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { + _waypoint_position_reached = true; + } + } else { + /* for normal mission items used their acceptance radius */ + 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 +MissionBlock::reset_mission_item_reached() +{ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; +} + +void +MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) +{ + sp->valid = true; + 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; + + switch (item->nav_cmd) { + case NAV_CMD_IDLE: + sp->type = SETPOINT_TYPE_IDLE; + break; + + case NAV_CMD_TAKEOFF: + sp->type = SETPOINT_TYPE_TAKEOFF; + break; + + case NAV_CMD_LAND: + sp->type = SETPOINT_TYPE_LAND; + break; + + case NAV_CMD_LOITER_TIME_LIMIT: + case NAV_CMD_LOITER_TURN_COUNT: + case NAV_CMD_LOITER_UNLIMITED: + sp->type = SETPOINT_TYPE_LOITER; + break; + + default: + sp->type = SETPOINT_TYPE_POSITION; + break; + } +} + +void +MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* reuse current setpoint as previous setpoint */ + 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) +{ + /* 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 */ + + _navigator->set_can_loiter_at_sp(true); + } + + 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; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/mission_block.h index b0f88e016..f99002752 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/mission_block.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 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 @@ -31,75 +31,76 @@ * ****************************************************************************/ /** - * @file navigator_mission.h - * Helper class to access missions + * @file mission_block.h * - * @author Julian Oes <joes@student.ethz.ch> + * Helper class to use mission items + * + * @author Julian Oes <julian@oes.ch> */ -#ifndef NAVIGATOR_MISSION_H -#define NAVIGATOR_MISSION_H +#ifndef NAVIGATOR_MISSION_BLOCK_H +#define NAVIGATOR_MISSION_BLOCK_H + +#include <drivers/drv_hrt.h> #include <uORB/topics/mission.h> -#include <uORB/topics/mission_result.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/position_setpoint_triplet.h> + +#include "navigator_mode.h" +class Navigator; -class __EXPORT Mission +class MissionBlock : public NavigatorMode { public: /** * Constructor */ - Mission(); + MissionBlock(Navigator *navigator, const char *name); /** - * Destructor, also kills the sensors task. + * Destructor */ - ~Mission(); - - void set_offboard_dataman_id(int new_id); - void set_current_offboard_mission_index(int new_index); - void set_current_onboard_mission_index(int new_index); - void set_offboard_mission_count(unsigned new_count); - void set_onboard_mission_count(unsigned new_count); - - void set_onboard_mission_allowed(bool allowed); - - bool current_mission_available(); - bool next_mission_available(); - - int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); - int get_next_mission_item(struct mission_item_s *mission_item); - - void move_to_next(); - - void report_mission_item_reached(); - void report_current_offboard_mission_item(); - void publish_mission_result(); + virtual ~MissionBlock(); -private: - bool current_onboard_mission_available(); - bool current_offboard_mission_available(); - bool next_onboard_mission_available(); - bool next_offboard_mission_available(); + /** + * 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(); - int _offboard_dataman_id; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items available */ - unsigned _onboard_mission_item_count; /** number of onboard mission items available */ + /** + * Convert a mission item to a position setpoint + * + * @param the mission item to convert + * @param the position setpoint that needs to be set + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - bool _onboard_mission_allowed; + /** + * Set previous position setpoint to current setpoint + */ + void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; + /** + * 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 + */ + bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); - int _mission_result_pub; + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; - struct mission_result_s _mission_result; + mission_item_s _mission_item; + bool _mission_item_valid; }; #endif diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index e1a6854b2..dd7f4c801 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size // float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; + return false; } void MissionFeasibilityChecker::updateNavigationCapabilities() { - int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } void MissionFeasibilityChecker::init() diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/mission_params.c index 476f93414..8692328db 100644 --- a/src/modules/navigator/navigator_state.h +++ b/src/modules/navigator/mission_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 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 @@ -32,24 +32,38 @@ ****************************************************************************/ /** - * @file navigator_state.h + * @file mission_params.c * - * Navigator state + * Parameters for mission. * - * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ -#ifndef NAVIGATOR_STATE_H_ -#define NAVIGATOR_STATE_H_ +#include <nuttx/config.h> -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; +#include <systemlib/param/param.h> -#endif /* NAVIGATOR_STATE_H_ */ +/* + * Mission parameters, accessible via MAVLink + */ + +/** + * Take-off altitude + * + * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to + * MIS_TAKEOFF_ALT on takeoff, then go to waypoint. + * + * @unit meters + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); + + +/** + * Enable onboard mission + * + * @min 0 + * @max 1 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 6ea9dec2b..a1e109030 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# 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 @@ -39,7 +39,13 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp \ + navigator_mode.cpp \ + mission_block.cpp \ + mission.cpp \ + mission_params.c \ + loiter.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..184ecd365 --- /dev/null +++ b/src/modules/navigator/navigator.h @@ -0,0 +1,219 @@ +/*************************************************************************** + * + * 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 <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#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 <uORB/topics/parameter_update.h> + +#include "navigator_mode.h" +#include "mission.h" +#include "loiter.h" +#include "rtl.h" +#include "geofence.h" + +/** + * Number of navigation modes that need on_active/on_inactive calls + * Currently: mission, loiter, and rtl + */ +#define NAVIGATOR_MODE_ARRAY_SIZE 3 + +class Navigator : public control::SuperBlock +{ +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); + + /** + * Setters + */ + void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } + + /** + * Getters + */ + 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; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } + int get_offboard_mission_sub() { return _offboard_mission_sub; } + Geofence& get_geofence() { return _geofence; } + bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } + float get_loiter_radius() { return _param_loiter_radius.get(); } + float get_acceptance_radius() { return _param_acceptance_radius.get(); } + int get_mavlink_fd() { return _mavlink_fd; } + +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 _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _onboard_mission_sub; /**< onboard mission subscription */ + int _offboard_mission_sub; /**< offboard mission subscription */ + int _param_update_sub; /**< param update 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 */ + + NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ + Mission _mission; /**< class that handles the missions */ + Loiter _loiter; /**< class that handles loiter */ + RTL _rtl; /**< class that handles RTL */ + + 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 */ + + control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ + control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ + /** + * 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(); + + /** + * Update parameters + */ + void params_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 06e0c5904..546602741 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -32,18 +32,18 @@ ****************************************************************************/ /** * @file navigator_main.cpp - * Implementation of the main navigation state machine. * - * Handles missions, geo fencing and failsafe navigation behavior. - * Published the mission item triplet for the position controller. + * Handles mission items, geo fencing and failsafe navigation behavior. + * Published the position setpoint triplet for the position controller. * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> + #include <stdio.h> #include <stdlib.h> #include <string.h> @@ -54,42 +54,28 @@ #include <poll.h> #include <time.h> #include <sys/ioctl.h> +#include <sys/types.h> +#include <sys/stat.h> + #include <drivers/device/device.h> #include <drivers/drv_hrt.h> #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> #include <uORB/topics/navigation_capabilities.h> -#include <systemlib/param/param.h> + #include <systemlib/err.h> -#include <systemlib/state_table.h> -#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <geo/geo.h> -#include <mathlib/mathlib.h> #include <dataman/dataman.h> +#include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include <sys/types.h> -#include <sys/stat.h> - -#include "navigator_state.h" -#include "navigator_mission.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 @@ -98,342 +84,53 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator : public StateTable -{ -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; - - 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 */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Geofence _geofence; - bool _geofence_violation_warning_sent; - - bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ - - struct navigation_capabilities_s _nav_caps; - - class Mission _mission; - - bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ - - MissionFeasibilityChecker missionFeasiblityChecker; - - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - - bool _pos_sp_triplet_updated; - - const char *nav_states_str[NAV_STATE_MAX]; - - struct { - float min_altitude; - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - float land_alt; - float rtl_alt; - float rtl_land_delay; - } _parameters; /**< local copies of parameters */ - - struct { - param_t min_altitude; - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - param_t land_alt; - param_t rtl_alt; - param_t rtl_land_delay; - } _parameter_handles; /**< handles for parameters */ - - enum Event { - EVENT_NONE_REQUESTED, - EVENT_READY_REQUESTED, - EVENT_LOITER_REQUESTED, - EVENT_MISSION_REQUESTED, - EVENT_RTL_REQUESTED, - EVENT_LAND_REQUESTED, - EVENT_MISSION_CHANGED, - EVENT_HOME_POSITION_CHANGED, - MAX_EVENT - }; - - /** - * State machine transition table - */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND - }; - - enum RTLState _rtl_state; - - /** - * 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(bool isrotaryWing); - - /** - * 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(); - - void publish_safepoints(unsigned points); - - /** - * Functions that are triggered when a new state is entered. - */ - void start_none(); - void start_ready(); - void start_loiter(); - void start_mission(); - void start_rtl(); - void start_land(); - void start_land_home(); - - /** - * Fork for state transitions - */ - void request_loiter_or_ready(); - void request_mission_if_available(); - - /** - * Guards offboard mission - */ - bool offboard_mission_available(unsigned relative_index); - - /** - * Guards onboard mission - */ - bool onboard_mission_available(unsigned relative_index); - - /** - * Reset all "reached" flags. - */ - void reset_reached(); - - /** - * Check if current mission item has been reached. - */ - bool check_mission_item_reached(); - - /** - * Perform actions when current mission item reached. - */ - void on_mission_item_reached(); - - /** - * Move to next waypoint - */ - void set_mission_item(); - - /** - * Switch to next RTL state - */ - void set_rtl_item(); - - /** - * Set position setpoint for mission item - */ - void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); - - /** - * Helper function to get a takeoff item - */ - void get_takeoff_setpoint(position_setpoint_s *pos_sp); - - /** - * Publish a new mission item triplet for position controller - */ - 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; } Navigator::Navigator() : - -/* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), - + SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), - -/* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _params_sub(-1), - _offboard_mission_sub(-1), - _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - -/* publications */ + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), _pos_sp_triplet_pub(-1), - -/* performance counters */ + _vstatus({}), + _control_mode({}), + _global_pos({}), + _home_pos({}), + _mission_item({}), + _nav_caps({}), + _pos_sp_triplet({}), + _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - + _geofence({}), _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission(), - _mission_item_valid(false), - _global_pos_valid(false), - _reset_loiter_pos(true), - _waypoint_position_reached(false), - _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _need_takeoff(true), - _do_takeoff(false), - _set_nav_state_timestamp(0), - _pos_sp_triplet_updated(false), -/* states */ - _rtl_state(RTL_STATE_NONE) + _navigation_mode(nullptr), + _mission(this, "MIS"), + _loiter(this, "LOI"), + _rtl(this, "RTL"), + _update_triplet(false), + _param_loiter_radius(this, "LOITER_RAD"), + _param_acceptance_radius(this, "ACC_RAD") { - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); - _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); - _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); - _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); - _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); - _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - - memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_item, 0, sizeof(struct mission_item_s)); - - memset(&nav_states_str, 0, sizeof(nav_states_str)); - nav_states_str[0] = "NONE"; - nav_states_str[1] = "READY"; - nav_states_str[2] = "LOITER"; - nav_states_str[3] = "MISSION"; - nav_states_str[4] = "RTL"; - nav_states_str[5] = "LAND"; - - /* Initialize state machine */ - myState = NAV_STATE_NONE; - start_none(); + /* Create a list of our possible navigation types */ + _navigation_mode_array[0] = &_mission; + _navigation_mode_array[1] = &_loiter; + _navigation_mode_array[2] = &_rtl; + + updateParams(); } Navigator::~Navigator() @@ -462,27 +159,6 @@ Navigator::~Navigator() } void -Navigator::parameters_update() -{ - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); - param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); - param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); - param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); - param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); - param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); - - _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); - - _geofence.updateParams(); -} - -void Navigator::global_position_update() { orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -500,56 +176,6 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } - -void -Navigator::offboard_mission_update(bool isrotaryWing) -{ - 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(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); - - _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - - _mission.set_offboard_mission_count(offboard_mission.count); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); - - } else { - _mission.set_offboard_mission_count(0); - _mission.set_current_offboard_mission_index(0); - } - - _mission.publish_mission_result(); -} - -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.set_current_onboard_mission_index(onboard_mission.current_index); - - } else { - _mission.set_onboard_mission_count(0); - _mission.set_current_onboard_mission_index(0); - } -} - void Navigator::vehicle_status_update() { @@ -570,6 +196,13 @@ Navigator::vehicle_control_mode_update() } void +Navigator::params_update() +{ + parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); +} + +void Navigator::task_main_trampoline(int argc, char *argv[]) { navigator::g_navigator->task_main(); @@ -580,16 +213,12 @@ Navigator::task_main() { /* inform about start */ warnx("Initializing.."); - fflush(stdout); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[navigator] started"); - /* 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) { @@ -603,68 +232,58 @@ 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)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); - parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(_vstatus.is_rotary_wing); - onboard_mission_update(); + params_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; hrt_abstime mavlink_open_time = 0; 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; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _global_pos_sub; + fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; - fds[2].fd = _home_pos_sub; + fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; - fds[3].fd = _capabilities_sub; + fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; - fds[4].fd = _offboard_mission_sub; + fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; - fds[5].fd = _onboard_mission_sub; + fds[5].fd = _param_update_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) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { + /* timed out - periodic check for _task_should_exit, etc. */ continue; - } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { + } else if (pret < 0) { + /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } @@ -677,162 +296,103 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* parameters updated */ + if (fds[5].revents & POLLIN) { + params_update(); + updateParams(); + } + /* vehicle control mode updated */ - if (fds[7].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[6].revents & POLLIN) { + if (fds[3].revents & POLLIN) { vehicle_status_update(); - - /* evaluate state requested by commander */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - /* publish position setpoint triplet on each status update if navigator active */ - _pos_sp_triplet_updated = true; - - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } - } - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } - - } else { - /* navigator shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); - } - - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - } - - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); - /* note that these new parameters won't be in effect until a mission triplet is published again */ } /* navigation capabilities updated */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } - /* offboard mission updated */ - if (fds[4].revents & POLLIN) { - offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed - dispatch(EVENT_MISSION_CHANGED); - } - - /* onboard mission updated */ - if (fds[5].revents & POLLIN) { - onboard_mission_update(); - // XXX check if mission really changed - dispatch(EVENT_MISSION_CHANGED); - } - /* home position updated */ - if (fds[2].revents & POLLIN) { + if (fds[1].revents & POLLIN) { home_position_update(); - // XXX check if home position really changed - dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ - if (fds[1].revents & POLLIN) { + if (fds[0].revents & POLLIN) { global_position_update(); - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - /* publish position setpoint triplet on each position update if navigator active */ - _pos_sp_triplet_updated = true; - - if (myState == NAV_STATE_LAND && !_global_pos_valid) { - /* got global position when landing, update setpoint */ - start_land(); - } - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } - } - /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { - //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } - } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } - _global_pos_valid = _vstatus.condition_global_position_valid; + /* Do stuff according to navigation state set by commander */ + switch (_vstatus.nav_state) { + case NAVIGATION_STATE_MANUAL: + case NAVIGATION_STATE_ACRO: + case NAVIGATION_STATE_ALTCTL: + case NAVIGATION_STATE_POSCTL: + _navigation_mode = nullptr; + _can_loiter_at_sp = false; + break; + case NAVIGATION_STATE_AUTO_MISSION: + _navigation_mode = &_mission; + break; + case NAVIGATION_STATE_AUTO_LOITER: + _navigation_mode = &_loiter; + break; + case NAVIGATION_STATE_AUTO_RTL: + _navigation_mode = &_rtl; + break; + case NAVIGATION_STATE_AUTO_RTGS: + _navigation_mode = &_rtl; /* TODO: change this to something else */ + break; + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + default: + _navigation_mode = nullptr; + _can_loiter_at_sp = false; + break; + } - /* publish position setpoint triplet if updated */ - if (_pos_sp_triplet_updated) { - _pos_sp_triplet_updated = false; - publish_position_setpoint_triplet(); + /* 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(); + } } - /* notify user about state changes */ - if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); - prevState = myState; + /* if nothing is running, set position setpoint triplet invalid */ + if (_navigation_mode == nullptr) { + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _update_triplet = true; + } + + if (_update_triplet) { + publish_position_setpoint_triplet(); + _update_triplet = false; } perf_end(_loop_perf); } - warnx("exiting."); _navigator_task = -1; @@ -863,19 +423,21 @@ Navigator::start() void Navigator::status() { - warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - - if (_global_pos_valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); - warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); - warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); - warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); - } + /* TODO: add this again */ + // warnx("Global position is %svalid", _global_pos_valid ? "" : "in"); + + // if (_global_pos.global_valid) { + // warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); + // warnx("Altitude %5.5f meters, altitude above home %5.5f meters", + // (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + // warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + // (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); + // } if (_fence_valid) { warnx("Geofence is valid"); + /* TODO: needed? */ // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); @@ -883,747 +445,19 @@ Navigator::status() } else { warnx("Geofence not set"); } - - switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - - default: - warnx("State: Unknown"); - break; - } -} - -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* NAV_STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - }, - { - /* NAV_STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - }, - { - /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - }, - { - /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, - }, - { - /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state - }, - { - /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - }, -}; - -void -Navigator::start_none() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - _rtl_state = RTL_STATE_NONE; - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_ready() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - - _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - - if (_rtl_state != RTL_STATE_DESCEND) { - /* reset RTL state if landed not at home */ - _rtl_state = RTL_STATE_NONE; - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_loiter() -{ - reset_reached(); - - _do_takeoff = false; - - /* set loiter position if needed */ - if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { - _reset_loiter_pos = false; - - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - - float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; - - /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { - _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - - } else { - _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - } - - } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; - _pos_sp_triplet.current.loiter_radius = _parameters.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; - _mission_item_valid = false; - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_mission() -{ - _need_takeoff = true; - - set_mission_item(); -} - -void -Navigator::set_mission_item() -{ - reset_reached(); - - /* copy current mission to previous item */ - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _reset_loiter_pos = true; - _do_takeoff = false; - - int ret; - bool onboard; - unsigned index; - - ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); - - if (ret == OK) { - _mission.report_current_offboard_mission_item(); - - _mission_item_valid = true; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _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) { - /* don't reset RTL state on RTL or LOITER items */ - _rtl_state = RTL_STATE_NONE; - } - - if (_vstatus.is_rotary_wing) { - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _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 - )) { - /* do special TAKEOFF handling for VTOL */ - _need_takeoff = false; - - /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _pos_sp_triplet.current.alt; - - if (_vstatus.condition_landed) { - /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ - takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); - } - - /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { - /* force TAKEOFF if landed or waypoint altitude is more than current */ - _do_takeoff = true; - - /* move current position setpoint to next */ - memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - /* set current setpoint to takeoff */ - - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = takeoff_alt_amsl; - _pos_sp_triplet.current.yaw = NAN; - _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; - } - - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - /* will need takeoff after landing */ - _need_takeoff = true; - } - } - - if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); - - } else { - if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); - - } else { - mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); - } - } - - } else { - /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_valid = false; - _pos_sp_triplet.current.valid = false; - warnx("ERROR: current WP can't be set"); - } - - if (!_do_takeoff) { - mission_item_s item_next; - ret = _mission.get_next_mission_item(&item_next); - - if (ret == OK) { - position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); - - } else { - /* this will fail for the last WP */ - _pos_sp_triplet.next.valid = false; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_rtl() -{ - _do_takeoff = false; - - /* decide if we need climb */ - if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { - _rtl_state = RTL_STATE_CLIMB; - - } else { - _rtl_state = RTL_STATE_RETURN; - } - } - - /* if switching directly to return state, reset altitude setpoint */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - } - - _reset_loiter_pos = true; - set_rtl_item(); -} - -void -Navigator::start_land() -{ - reset_reached(); - - /* this state can be requested by commander even if no global position available, - * in his case controller must perform landing without position control */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::start_land_home() -{ - reset_reached(); - - /* land to home position, should be called when hovering above home, from RTL state */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::set_rtl_item() -{ - reset_reached(); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - } - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); - break; - } - - case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // don't change altitude - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - default: { - mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::request_loiter_or_ready() -{ - /* XXX workaround: no landing detector for fixedwing yet */ - if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } -} - -void -Navigator::request_mission_if_available() -{ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - request_loiter_or_ready(); - } -} - -void -Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) -{ - sp->valid = true; - - if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* set home position for RTL item */ - sp->lat = _home_pos.lat; - sp->lon = _home_pos.lon; - sp->alt = _home_pos.alt + _parameters.rtl_alt; - - 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); - } - sp->loiter_radius = _parameters.loiter_radius; - sp->loiter_direction = 1; - sp->pitch_min = 0.0f; - - } 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; - } -} - -bool -Navigator::check_mission_item_reached() -{ - /* only check if there is actually a mission item to check */ - if (!_mission_item_valid) { - return false; - } - - if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; - } - - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { - - return false; - } - - uint64_t now = hrt_absolute_time(); - - if (!_waypoint_position_reached) { - float acceptance_radius; - - if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item.acceptance_radius; - - } else { - acceptance_radius = _parameters.acceptance_radius; - } - - if (_do_takeoff) { - /* require only altitude for takeoff */ - if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { - _waypoint_position_reached = true; - } - - } else { - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - - if (dist >= 0.0f && dist <= acceptance_radius) { - _waypoint_position_reached = true; - } - } - } - - if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { - /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - - if (fabsf(yaw_err) < 0.2f) { /* XXX 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 >= (uint64_t)_mission_item.time_inside * 1e6) - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - reset_reached(); - return true; - } - } - - return false; - -} - -void -Navigator::reset_reached() -{ - _time_first_inside_orbit = 0; - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - -} - -void -Navigator::on_mission_item_reached() -{ - if (myState == NAV_STATE_MISSION) { - - _mission.report_mission_item_reached(); - - if (_do_takeoff) { - /* takeoff completed */ - _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); - - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - if (_mission.current_mission_available()) { - if (_mission_item.autocontinue) { - /* continue mission */ - set_mission_item(); - - } else { - /* autocontinue disabled for this item */ - request_loiter_or_ready(); - } - - } else { - /* if no more mission items available then finish mission */ - /* loiter at last waypoint */ - _reset_loiter_pos = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - request_loiter_or_ready(); - } - - } else if (myState == NAV_STATE_RTL) { - /* RTL completed */ - if (_rtl_state == RTL_STATE_DESCEND) { - /* hovering above home position, land if needed or loiter */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - - if (_mission_item.autocontinue) { - dispatch(EVENT_LAND_REQUESTED); - - } else { - _reset_loiter_pos = false; - dispatch(EVENT_LOITER_REQUESTED); - } - - } else { - /* next RTL step */ - _rtl_state = (RTLState)(_rtl_state + 1); - set_rtl_item(); - } - - } else if (myState == NAV_STATE_LAND) { - /* landing completed */ - mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); - dispatch(EVENT_READY_REQUESTED); - } - _mission.publish_mission_result(); } void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - _pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState); + /* TODO: set nav_state */ /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { - /* advertise and publish */ _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp deleted file mode 100644 index 49fc62785..000000000 --- a/src/modules/navigator/navigator_mission.cpp +++ /dev/null @@ -1,319 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 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_mission.cpp - * Helper class to access missions - * - * @author Julian Oes <joes@student.ethz.ch> - */ - -#include <string.h> -#include <stdlib.h> -#include <dataman/dataman.h> -#include <systemlib/err.h> -#include <uORB/uORB.h> -#include <uORB/topics/mission_result.h> -#include "navigator_mission.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - - -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_result_pub(-1) -{ - memset(&_mission_result, 0, sizeof(struct mission_result_s)); -} - -Mission::~Mission() -{ - -} - -void -Mission::set_offboard_dataman_id(int new_id) -{ - _offboard_dataman_id = new_id; -} - -void -Mission::set_current_offboard_mission_index(int new_index) -{ - if (new_index != -1) { - warnx("specifically set to %d", new_index); - _current_offboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } - } - report_current_offboard_mission_item(); -} - -void -Mission::set_current_onboard_mission_index(int new_index) -{ - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } - } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); -} - -void -Mission::set_offboard_mission_count(unsigned new_count) -{ - _offboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_count(unsigned new_count) -{ - _onboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; -} - -bool -Mission::current_mission_available() -{ - return (current_onboard_mission_available() || current_offboard_mission_available()); - -} - -bool -Mission::next_mission_available() -{ - return (next_onboard_mission_available() || next_offboard_mission_available()); -} - -int -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) -{ - /* try onboard mission first */ - if (current_onboard_mission_available()) { - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - /* otherwise fallback to offboard */ - - } else if (current_offboard_mission_available()) { - - 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; - } - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _current_mission_type = MISSION_TYPE_NONE; - return ERROR; - } - - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - - } else { - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; - return ERROR; - } - - return OK; -} - -int -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) -{ - /* try onboard mission first */ - if (next_onboard_mission_available()) { - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* otherwise fallback to offboard */ - - } else if (next_offboard_mission_available()) { - - 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; - } - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - } else { - /* happens when no more mission items can be added as a next item */ - return ERROR; - } - - return OK; -} - - -bool -Mission::current_onboard_mission_available() -{ - return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; -} - -bool -Mission::current_offboard_mission_available() -{ - return _offboard_mission_item_count > _current_offboard_mission_index; -} - -bool -Mission::next_onboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_ONBOARD) { - next = 1; - } - - return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; -} - -bool -Mission::next_offboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_OFFBOARD) { - next = 1; - } - - return _offboard_mission_item_count > (_current_offboard_mission_index + next); -} - -void -Mission::move_to_next() -{ - 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) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; - } -} - -void -Mission::report_current_offboard_mission_item() -{ - _mission_result.index_current_mission = _current_offboard_mission_index; -} - -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.mission_reached = false; -} diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp new file mode 100644 index 000000000..25e767c2b --- /dev/null +++ b/src/modules/navigator/navigator_mode.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 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_mode.cpp + * + * Helper class for different modes in navigator + * + * @author Julian Oes <julian@oes.ch> + */ + +#include "navigator_mode.h" + +NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : + SuperBlock(NULL, name), + _navigator(navigator), + _first_run(true) +{ + /* load initial params */ + updateParams(); + /* set initial mission items */ + on_inactive(); +} + +NavigatorMode::~NavigatorMode() +{ +} + +void +NavigatorMode::on_inactive() +{ + _first_run = true; +} + +bool +NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + pos_sp_triplet->current.valid = false; + _first_run = false; + return false; +} diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h new file mode 100644 index 000000000..cbb53d91b --- /dev/null +++ b/src/modules/navigator/navigator_mode.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 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_mode.h + * + * Helper class for different modes in navigator + * + * @author Julian Oes <julian@oes.ch> + */ + +#ifndef NAVIGATOR_MODE_H +#define NAVIGATOR_MODE_H + +#include <drivers/drv_hrt.h> + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <dataman/dataman.h> + +#include <uORB/topics/position_setpoint_triplet.h> + +class Navigator; + +class NavigatorMode : public control::SuperBlock +{ +public: + /** + * Constructor + */ + NavigatorMode(Navigator *navigator, const char *name); + + /** + * Destructor + */ + virtual ~NavigatorMode(); + + /** + * 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 + */ + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + +protected: + Navigator *_navigator; + bool _first_run; +}; + +#endif diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 5139283b6..084afe340 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 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 @@ -17,7 +17,7 @@ * 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 + * 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, @@ -34,104 +34,33 @@ /** * @file navigator_params.c * - * Parameters defined by the navigator task. + * Parameters for navigator in general * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <julian@oes.ch> */ #include <nuttx/config.h> #include <systemlib/param/param.h> -/* - * Navigator parameters, accessible via MAVLink - */ - -/** - * Minimum altitude (fixed wing only) - * - * Minimum altitude above home for LOITER. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); - -/** - * Waypoint acceptance radius - * - * Default value of acceptance radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); - /** - * Loiter radius (fixed wing only) + * Loiter radius (FW only) * - * Default value of loiter radius (if not specified in mission item). + * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * * @unit meters * @min 0.0 - * @group Navigation + * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** - * Enable onboard mission - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); - -/** - * Take-off altitude - * - * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); - -/** - * Landing altitude - * - * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); - -/** - * Return-To-Launch altitude + * Acceptance Radius * - * Minimum altitude above home position for going home in RTL mode. + * Default acceptance radius, overridden by acceptance radius of waypoint if set. * * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); - -/** - * Return-To-Launch delay - * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. - * - * @unit seconds - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); - -/** - * Enable parachute deployment - * - * @group Navigation + * @min 1.0 + * @group Mission */ -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp new file mode 100644 index 000000000..597a2c0ec --- /dev/null +++ b/src/modules/navigator/rtl.cpp @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * 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_rtl.cpp + * Helper class to access RTL + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> + +#include "navigator.h" +#include "rtl.h" + +#define DELAY_SIGMA 0.01f + +RTL::RTL(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _rtl_state(RTL_STATE_NONE), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY") +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +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) +{ + bool updated = false; + + if (_first_run) { + _first_run = 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 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; + } + } + + 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; +} + +void +RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) +{ + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_RETURN: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude + + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _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; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_LOITER: { + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + + if (autoland) { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + } + break; + } + + case RTL_STATE_LAND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_LANDED: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + break; + } + + default: + break; + } + + /* 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; +} + +void +RTL::advance_rtl() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + _rtl_state = RTL_STATE_LOITER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LOITER: + _rtl_state = RTL_STATE_LAND; + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h new file mode 100644 index 000000000..b4b729e89 --- /dev/null +++ b/src/modules/navigator/rtl.h @@ -0,0 +1,110 @@ +/*************************************************************************** + * + * 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_rtl.h + * Helper class for RTL + * + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef NAVIGATOR_RTL_H +#define NAVIGATOR_RTL_H + +#include <controllib/blocks.hpp> +#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> + +#include "navigator_mode.h" +#include "mission_block.h" + +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(); + + /** + * 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); + + +private: + /** + * Set the RTL item + */ + void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); + + /** + * Move to next RTL item + */ + void advance_rtl(); + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_LAND, + RTL_STATE_LANDED, + } _rtl_state; + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c new file mode 100644 index 000000000..bfe6ce7e1 --- /dev/null +++ b/src/modules/navigator/rtl_params.c @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 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 rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); |