diff options
author | ilya <ilja@Ilyas-MacBook-Pro.local> | 2014-10-26 20:29:05 +0200 |
---|---|---|
committer | ilya <ilja@Ilyas-MacBook-Pro.local> | 2014-10-26 20:29:05 +0200 |
commit | 1d31da285505d2c035ece77461f4e4f005c7ea60 (patch) | |
tree | 16fea57547bd2ec9386df8b41a72c78d5c97555e /src/modules | |
parent | 8eec07deb345b56ba4da75d77417e6d4738eded0 (diff) | |
download | px4-firmware-1d31da285505d2c035ece77461f4e4f005c7ea60.tar.gz px4-firmware-1d31da285505d2c035ece77461f4e4f005c7ea60.tar.bz2 px4-firmware-1d31da285505d2c035ece77461f4e4f005c7ea60.zip |
Changes in navigator module
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/abs_follow.cpp | 257 | ||||
-rw-r--r-- | src/modules/navigator/abs_follow.h | 78 | ||||
-rw-r--r-- | src/modules/navigator/abs_follow_params.c | 65 | ||||
-rw-r--r-- | src/modules/navigator/loiter.cpp | 469 | ||||
-rw-r--r-- | src/modules/navigator/loiter.h | 32 | ||||
-rw-r--r-- | src/modules/navigator/loiter_params.c | 63 | ||||
-rw-r--r-- | src/modules/navigator/module.mk | 6 | ||||
-rw-r--r-- | src/modules/navigator/navigator.h | 28 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 50 | ||||
-rw-r--r-- | src/modules/navigator/navigator_mode.cpp | 191 | ||||
-rw-r--r-- | src/modules/navigator/navigator_mode.h | 76 | ||||
-rw-r--r-- | src/modules/navigator/navigator_mode_params.c | 109 | ||||
-rw-r--r-- | src/modules/navigator/navigator_params.c | 10 | ||||
-rw-r--r-- | src/modules/navigator/rtl.cpp | 362 | ||||
-rw-r--r-- | src/modules/navigator/rtl.h | 31 | ||||
-rw-r--r-- | src/modules/navigator/rtl_params.c | 56 |
16 files changed, 1578 insertions, 305 deletions
diff --git a/src/modules/navigator/abs_follow.cpp b/src/modules/navigator/abs_follow.cpp new file mode 100644 index 000000000..cce2b712a --- /dev/null +++ b/src/modules/navigator/abs_follow.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * 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 abs_follow.cpp + * + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Martins Frolovs <martins.f@airdog.com> + */ + +#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 "navigator.h" +#include "abs_follow.h" + +AbsFollow::AbsFollow(Navigator *navigator, const char *name) : + MissionBlock(navigator, name) +{ + updateParameters(); +} + +AbsFollow::~AbsFollow() +{ +} + +void +AbsFollow::on_inactive() +{ +} + +void +AbsFollow::on_activation() +{ + updateParameters(); + + _afollow_offset.zero(); + global_pos = _navigator->get_global_position(); + target_pos = _navigator->get_target_position(); + home_pos = _navigator->get_home_position(); + pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + _init_alt = global_pos->alt; + + float target_alt = target_pos->alt; + + get_vector_to_next_waypoint_fast(target_pos->lat, target_pos->lon, global_pos->lat, global_pos->lon, &_afollow_offset.data[0], &_afollow_offset.data[1]); + _afollow_offset.data[2] = -(global_pos->alt - target_alt); + + mavlink_log_info(_navigator->get_mavlink_fd(), "[abs_fol] Reset abs_follow offset: %.2f, %.2f, %.2f", (double)_afollow_offset(0), (double)_afollow_offset(1), (double)_afollow_offset(2)); + + point_camera_to_target(&(pos_sp_triplet->current)); + _navigator->set_position_setpoint_triplet_updated(); +} + +void +AbsFollow::on_active() +{ + + /* Update position data pointer */ + global_pos = _navigator->get_global_position(); + home_pos = _navigator->get_home_position(); + target_pos = _navigator->get_target_position(); + pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Execute command if received + if ( update_vehicle_command() ) + execute_vehicle_command(); + + _target_lat = target_pos->lat; + _target_lon = target_pos->lon; + + double lat_new; + double lon_new; + + /* add offset to target position */ + add_vector_to_global_position( + _target_lat, _target_lon, + _afollow_offset(0), _afollow_offset(1), + &lat_new, &lon_new + ); + + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.type = SETPOINT_TYPE_MOVING; + + pos_sp_triplet->current.lat = lat_new; + pos_sp_triplet->current.lon = lon_new; + + if (_parameters.afol_rep_target_alt) + pos_sp_triplet->current.alt = target_pos->alt - _afollow_offset(2); + else + pos_sp_triplet->current.alt = _init_alt; + + + /* calculate direction to target */ + pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(global_pos->lat, global_pos->lon, target_pos->lat, target_pos->lon); + pos_sp_triplet->current.pitch_min = 0.0f; + + _navigator->set_position_setpoint_triplet_updated(); + +} + +void +AbsFollow::execute_vehicle_command() { + + // Update _parameter values with the latest navigator_mode parameters + memcpy(&_parameters, &(NavigatorMode::_parameters), sizeof(_parameters)); + + vehicle_command_s cmd = _vcommand; + + if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) { + + REMOTE_CMD remote_cmd = (REMOTE_CMD)cmd.param1; + + math::Vector<3> offset =_afollow_offset; + + switch(remote_cmd){ + case REMOTE_CMD_PLAY_PAUSE: { + commander_request_s *commander_request = _navigator->get_commander_request(); + commander_request->request_type = V_MAIN_STATE_CHANGE; + commander_request->main_state = MAIN_STATE_AUTO_LOITER; + _navigator->set_commander_request_updated(); + break; + } + + case REMOTE_CMD_UP: { + + _afollow_offset.data[2] -= _parameters.loi_step_len; + break; + } + case REMOTE_CMD_DOWN: { + + _afollow_offset.data[2] += _parameters.loi_step_len; + break; + } + case REMOTE_CMD_LEFT: { + + math::Matrix<3, 3> R_phi; + + double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1)); + + // derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) * 2PI + // radius: (sqrt(x^2 + y^2) + // circumference C: (radius * 2* PI) + // step length fraction of C: step/C + // angle of step fraction in radians: step/C * 2PI + double alpha = (double)_parameters.loi_step_len / radius; + + // vector yaw rotation +alpha or -alpha depending on left or right + R_phi.from_euler(0.0f, 0.0f, alpha); + math::Vector<3> offset_new = R_phi * offset; + + _afollow_offset = offset_new; + + break; + } + case REMOTE_CMD_RIGHT: { + + math::Matrix<3, 3> R_phi; + + double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1)); + + // derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) * 2PI + // radius: (sqrt(x^2 + y^2) + // circumference C: (radius * 2* PI) + // step length fraction of C: step/C + // angle of step fraction in radians: step/C * 2PI + double alpha = (double)_parameters.loi_step_len / radius; + + // vector yaw rotation +alpha or -alpha depending on left or right + R_phi.from_euler(0.0f, 0.0f, -alpha); + math::Vector<3> offset_new = R_phi * offset; + + _afollow_offset = offset_new; + + break; + } + case REMOTE_CMD_CLOSER: { + + // Calculate vector angle from target to device with atan2(y, x) + float alpha = atan2f(offset(1), offset(0)); + + // Create vector in the same direction, with loiter_step length + math::Vector<3> offset_delta( + cosf(alpha) * _parameters.loi_step_len, + sinf(alpha) * _parameters.loi_step_len, + 0); + + math::Vector<3> offset_new = offset - offset_delta; + + _afollow_offset = offset_new; + + break; + + } + + case REMOTE_CMD_FURTHER: { + + // Calculate vector angle from target to device with atan2(y, x) + float alpha = atan2(offset(1), offset(0)); + + // Create vector in the same direction, with loiter_step length + math::Vector<3> offset_delta( + cosf(alpha) * _parameters.loi_step_len, + sinf(alpha) * _parameters.loi_step_len, + 0); + + math::Vector<3> offset_new = offset + offset_delta; + + _afollow_offset = offset_new; + + break; + } + + } + } + +} diff --git a/src/modules/navigator/abs_follow.h b/src/modules/navigator/abs_follow.h new file mode 100644 index 000000000..47d4383f0 --- /dev/null +++ b/src/modules/navigator/abs_follow.h @@ -0,0 +1,78 @@ +/*************************************************************************** + * + * 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 abs_follow.h + * + * @author Edgars Simanovskis <es@robotics.lv> + * @author Martins Frolovs <martins.f@airdog.com> + */ + +#ifndef NAVIGATOR_ABS_FOLLOW_H +#define NAVIGATOR_ABS_FOLLOW_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/uORB.h> + +#include "navigator_mode.h" +#include "mission_block.h" + +class AbsFollow : public MissionBlock +{ +public: + AbsFollow(Navigator *navigator, const char *name); + + ~AbsFollow(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + + virtual void execute_vehicle_command(); + +private: + + math::Vector<3> _afollow_offset; /**< offset from target for AFOLLOW mode */ + double _target_lat; /**< prediction for target latitude */ + double _target_lon; /**< prediction for target longitude */ + float _target_alt; /**< prediction for target altitude */ + float _init_alt; /**< initial altitude on abs follow start > */ + math::Vector<3> _target_vel; /**< target velocity vector */ + hrt_abstime _t_prev; + +}; + +#endif diff --git a/src/modules/navigator/abs_follow_params.c b/src/modules/navigator/abs_follow_params.c new file mode 100644 index 000000000..575df858e --- /dev/null +++ b/src/modules/navigator/abs_follow_params.c @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 abs_follow_params.c + * + * Parameter for absolute follow mode. + * + * @author Martins Frolovs <martins.f@airdog.com> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + + +/** + * Use camera pitch + * + * Used in Abs_follow mode + * + * 0: Camera pitch is still + * 1: Camera pitch follows target + * @group Navigator + */ +PARAM_DEFINE_INT32(AFOL_USE_CAM_P, 1); + + +/** + * Drone repeat target altitude changes + * + * 0: Drone don't repeat target's altitude change + * 1: Drone repeat target's altitude changes + * @group Navigator + */ +PARAM_DEFINE_INT32(AFOL_REP_TALT, 1); diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f827e70c9..46c81523f 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -37,6 +37,7 @@ * * @author Julian Oes <julian@oes.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Martins Frolovs <martins.f@airdog.vom> */ #include <string.h> @@ -44,6 +45,7 @@ #include <stdbool.h> #include <math.h> #include <fcntl.h> +#include <stdio.h> #include <mavlink/mavlink_log.h> #include <systemlib/err.h> @@ -57,8 +59,7 @@ Loiter::Loiter(Navigator *navigator, const char *name) : MissionBlock(navigator, name) { - /* load initial params */ - updateParams(); + updateParameters(); } Loiter::~Loiter() @@ -73,21 +74,471 @@ Loiter::on_inactive() void Loiter::on_activation() { - /* set current mission item to loiter */ - set_loiter_item(&_mission_item); + updateParameters(); - /* convert mission item to current setpoint */ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + // Determine current loiter sub mode + struct vehicle_status_s *vstatus = _navigator->get_vstatus(); + + pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + _mavlink_fd = _navigator->get_mavlink_fd(); + + if (vstatus->condition_landed) { + set_sub_mode(LOITER_SUB_MODE_LANDED); + } + else { + set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT); + } +} + +void +Loiter::on_active() +{ + target_pos = _navigator->get_target_position(); + global_pos = _navigator->get_global_position(); + pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + + if (loiter_sub_mode == LOITER_SUB_MODE_TAKING_OFF && check_current_pos_sp_reached()) { + set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT); + } + + if (loiter_sub_mode == LOITER_SUB_MODE_LANDING && check_current_pos_sp_reached()) { + set_sub_mode(LOITER_SUB_MODE_LANDED); + + disarm(); + } + + if (loiter_sub_mode == LOITER_SUB_MODE_COME_TO_ME && check_current_pos_sp_reached()) { + set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT); + } + + + if ( (loiter_sub_mode == LOITER_SUB_MODE_AIM_AND_SHOOT) && (target_pos->alt + _parameters.loi_min_alt > pos_sp_triplet->current.alt)){ + pos_sp_triplet->current.alt = target_pos->alt + _parameters.loi_min_alt; + } + + if ( update_vehicle_command() ) + execute_vehicle_command(); + + if (loiter_sub_mode == LOITER_SUB_MODE_AIM_AND_SHOOT || loiter_sub_mode == LOITER_SUB_MODE_COME_TO_ME) + point_camera_to_target(&(pos_sp_triplet->current)); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Loiter::execute_vehicle_command() +{ + updateParams(); + + vehicle_command_s cmd = _vcommand; + + switch (loiter_sub_mode){ + case LOITER_SUB_MODE_LANDED: + execute_command_in_landed(cmd); + break; + case LOITER_SUB_MODE_AIM_AND_SHOOT: + execute_command_in_aim_and_shoot(cmd); + break; + case LOITER_SUB_MODE_LOOK_DOWN: + execute_command_in_look_down(cmd); + break; + case LOITER_SUB_MODE_COME_TO_ME: + execute_command_in_come_to_me(cmd); + break; + case LOITER_SUB_MODE_LANDING: + execute_command_in_landing(cmd); + break; + case LOITER_SUB_MODE_TAKING_OFF: + execute_command_in_taking_off(cmd); + break; + } + +} + +void +Loiter::execute_command_in_landed(vehicle_command_s cmd){ + + if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) { + + int remote_cmd = cmd.param1; + + if (remote_cmd == REMOTE_CMD_TAKEOFF) { + set_sub_mode(LOITER_SUB_MODE_TAKING_OFF); + takeoff(); + } else if (remote_cmd == REMOTE_CMD_LAND_DISARM) { + disarm(); + } + } +} + +void +Loiter::execute_command_in_aim_and_shoot(vehicle_command_s cmd){ + + + + // Calculate offset + float offset_x; + float offset_y; + float offset_z = target_pos->alt - global_pos->alt; + + get_vector_to_next_waypoint( + target_pos->lat, + target_pos->lon, + global_pos->lat, + global_pos->lon, + &offset_x, + &offset_y + ); + + + + math::Vector<3> offset(offset_x, offset_y, offset_z); + + + if (cmd.command == VEHICLE_CMD_DO_SET_MODE){ + + //uint8_t base_mode = (uint8_t)cmd.param1; + uint8_t main_mode = (uint8_t)cmd.param2; + + if (main_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) { + // Make request to COMMANDER do change state to RTL + commander_request_s *commander_request = _navigator->get_commander_request(); + commander_request->request_type = V_MAIN_STATE_CHANGE; + commander_request->main_state = MAIN_STATE_AUTO_RTL; + _navigator->set_commander_request_updated(); + + } + } + + + if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) { + + REMOTE_CMD remote_cmd = (REMOTE_CMD)cmd.param1; + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; + + switch(remote_cmd){ + case REMOTE_CMD_LAND_DISARM: { + land(); + set_sub_mode(LOITER_SUB_MODE_LANDING); + break; + } + case REMOTE_CMD_UP: { + + pos_sp_triplet->current.alt = global_pos->alt + _parameters.loi_step_len; + pos_sp_triplet->current.lat = global_pos->lat; + pos_sp_triplet->current.lon = global_pos->lon; + + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + break; + } + case REMOTE_CMD_DOWN: { + + pos_sp_triplet->current.alt = global_pos->alt - _parameters.loi_step_len; + pos_sp_triplet->current.lat = global_pos->lat; + pos_sp_triplet->current.lon = global_pos->lon; + + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + break; + } + case REMOTE_CMD_LEFT: { + + math::Matrix<3, 3> R_phi; + + double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1)); + + // derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) * 2PI + // radius: (sqrt(x^2 + y^2) + // circumference C: (radius * 2* PI) + // step length fraction of C: step/C + // angle of step fraction in radians: step/C * 2PI + double alpha = (double)_parameters.loi_step_len / radius; + + // vector yaw rotation +alpha or -alpha depending on left or right + R_phi.from_euler(0.0f, 0.0f, -alpha); + math::Vector<3> offset_new = R_phi * offset; + + double lat_new, lon_new; + add_vector_to_global_position( + (*target_pos).lat, + (*target_pos).lon, + offset_new(0), + offset_new(1), + &lat_new, + &lon_new + ); + + pos_sp_triplet->current.lat = lat_new; + pos_sp_triplet->current.lon = lon_new; + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + break; + } + case REMOTE_CMD_RIGHT: { + + math::Matrix<3, 3> R_phi; + + double radius = sqrt(offset(0) * offset(0) + offset(1) * offset(1)); + + // derived from formula: ( step / ( sqrt(x^2 + y^2)*2PI ) ) * 2PI + // radius: (sqrt(x^2 + y^2) + // circumference C: (radius * 2* PI) + // step length fraction of C: step/C + // angle of step fraction in radians: step/C * 2PI + double alpha = (double)_parameters.loi_step_len / radius; + + // vector yaw rotation +alpha or -alpha depending on left or right + R_phi.from_euler(0.0f, 0.0f, +alpha); + math::Vector<3> offset_new = R_phi * offset; + + double lat_new, lon_new; + add_vector_to_global_position( + (*target_pos).lat, + (*target_pos).lon, + offset_new(0), + offset_new(1), + &lat_new, + &lon_new + ); + + pos_sp_triplet->current.lat = lat_new; + pos_sp_triplet->current.lon = lon_new; + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + break; + } + case REMOTE_CMD_CLOSER: { + + // Calculate vector angle from target to device with atan2(y, x) + float alpha = atan2f(offset(1), offset(0)); + + // Create vector in the same direction, with loiter_step length + math::Vector<3> offset_delta( + cosf(alpha) * _parameters.loi_step_len, + sinf(alpha) * _parameters.loi_step_len, + 0); + + math::Vector<3> offset_new = offset - offset_delta; + + double lat_new, lon_new; + add_vector_to_global_position( + (*target_pos).lat, + (*target_pos).lon, + offset_new(0), + offset_new(1), + &lat_new, + &lon_new + ); + + pos_sp_triplet->current.lat = lat_new; + pos_sp_triplet->current.lon = lon_new; + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + break; + + } + + case REMOTE_CMD_FURTHER: { + + // Calculate vector angle from target to device with atan2(y, x) + float alpha = atan2(offset(1), offset(0)); + + // Create vector in the same direction, with loiter_step length + math::Vector<3> offset_delta( + cosf(alpha) * _parameters.loi_step_len, + sinf(alpha) * _parameters.loi_step_len, + 0); + + math::Vector<3> offset_new = offset + offset_delta; + + double lat_new, lon_new; + add_vector_to_global_position( + target_pos->lat, + target_pos->lon, + offset_new(0), + offset_new(1), + &lat_new, + &lon_new + ); + + pos_sp_triplet->current.lat = lat_new; + pos_sp_triplet->current.lon = lon_new; + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + break; + } + case REMOTE_CMD_COME_TO_ME: { + + pos_sp_triplet->current.lat = target_pos->lat; + pos_sp_triplet->current.lon = target_pos->lon; + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + set_sub_mode(LOITER_SUB_MODE_COME_TO_ME); + + break; + } + case REMOTE_CMD_LOOK_DOWN: { + + pos_sp_triplet->current.camera_pitch = -1.57f; + set_sub_mode(LOITER_SUB_MODE_LOOK_DOWN); + break; + + } + case REMOTE_CMD_PLAY_PAUSE: { + + commander_request_s *commander_request = _navigator->get_commander_request(); + commander_request->request_type = V_MAIN_STATE_CHANGE; + commander_request->main_state = MAIN_STATE_AUTO_ABS_FOLLOW; + _navigator->set_commander_request_updated(); + + break; + } + + } + + } + + _navigator->set_position_setpoint_triplet_updated(); + +} + +void +Loiter::execute_command_in_look_down(vehicle_command_s cmd){ + + if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) { + int remote_cmd = cmd.param1; + if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) { + set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT); + } + } +} + +void +Loiter::execute_command_in_come_to_me(vehicle_command_s cmd){ + + if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) { + int remote_cmd = cmd.param1; + if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) { + set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT); + } + } + +} + +void +Loiter::execute_command_in_landing(vehicle_command_s cmd){ + + if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) { + int remote_cmd = cmd.param1; + if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) { + set_sub_mode(LOITER_SUB_MODE_AIM_AND_SHOOT); + loiter_sub_mode = LOITER_SUB_MODE_AIM_AND_SHOOT; + } + } +} + +void +Loiter::set_sub_mode(LOITER_SUB_MODE new_sub_mode){ + + if (new_sub_mode == LOITER_SUB_MODE_AIM_AND_SHOOT) { + + // Reset setpoint position to current global position + global_pos = _navigator->get_global_position(); + + 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_POSITION; + pos_sp_triplet->current.alt = global_pos->alt; + pos_sp_triplet->current.lon = global_pos->lon; + pos_sp_triplet->current.lat = global_pos->lat; + + _navigator->set_position_setpoint_triplet_updated(); + + } + + loiter_sub_mode = new_sub_mode; + + char * sub_mode_str; + + switch(new_sub_mode){ + case LOITER_SUB_MODE_AIM_AND_SHOOT: + sub_mode_str = "Aim-and-shoot"; + break; + case LOITER_SUB_MODE_LOOK_DOWN: + sub_mode_str = "Look down"; + break; + case LOITER_SUB_MODE_COME_TO_ME: + sub_mode_str = "Come-to-me"; + break; + case LOITER_SUB_MODE_LANDING: + sub_mode_str = "Landing"; + break; + case LOITER_SUB_MODE_TAKING_OFF: + sub_mode_str = "Taking-off"; + break; + case LOITER_SUB_MODE_LANDED: + sub_mode_str = "Landed"; + break; + } + + mavlink_log_info(_mavlink_fd, "[loiter] Loiter sub mode set to %s ! ", sub_mode_str); + +} + +void +Loiter::execute_command_in_taking_off(vehicle_command_s cmd) { +} + +void +Loiter::takeoff() +{ pos_sp_triplet->previous.valid = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + pos_sp_triplet->current.lat = global_pos->lat; + pos_sp_triplet->current.lon = global_pos->lon; + pos_sp_triplet->current.alt = global_pos->alt + _parameters.takeoff_alt; + + int toa = (int)_parameters.takeoff_alt; + + mavlink_log_info(_navigator->get_mavlink_fd(), "Use takeoff alt: %d", toa); + + pos_sp_triplet->current.yaw = NAN; + pos_sp_triplet->current.type = SETPOINT_TYPE_TAKEOFF; _navigator->set_position_setpoint_triplet_updated(); + } void -Loiter::on_active() +Loiter::land() +{ + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; + + pos_sp_triplet->current.lat = global_pos->lat; + pos_sp_triplet->current.lon = global_pos->lon; + pos_sp_triplet->current.alt = global_pos->alt; + pos_sp_triplet->current.yaw = NAN; + pos_sp_triplet->current.type = SETPOINT_TYPE_LAND; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Loiter::disarm() { + commander_request_s *commander_request = _navigator->get_commander_request(); + commander_request->request_type = V_DISARM; + _navigator->set_commander_request_updated(); } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 37ab57a07..2dcb4c39b 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -41,8 +41,11 @@ #ifndef NAVIGATOR_LOITER_H #define NAVIGATOR_LOITER_H +#include <mathlib/mathlib.h> +#include <geo/geo.h> #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> +#include <uORB/topics/vehicle_command.h> #include "navigator_mode.h" #include "mission_block.h" @@ -59,6 +62,35 @@ public: virtual void on_activation(); virtual void on_active(); + + virtual void execute_vehicle_command(); + +private: + + void takeoff(); + void land(); + void disarm(); + + enum LOITER_SUB_MODE { + LOITER_SUB_MODE_LANDED = 0, // vehicle on ground + LOITER_SUB_MODE_AIM_AND_SHOOT, // aim and shoot + LOITER_SUB_MODE_LOOK_DOWN, // look down + LOITER_SUB_MODE_COME_TO_ME, // come to me + LOITER_SUB_MODE_LANDING, // vehicle is landing + LOITER_SUB_MODE_TAKING_OFF, // vehicle is taking off + + } loiter_sub_mode; + + void execute_command_in_landed(vehicle_command_s cmd); + void execute_command_in_aim_and_shoot(vehicle_command_s cmd); + void execute_command_in_look_down(vehicle_command_s cmd); + void execute_command_in_come_to_me(vehicle_command_s cmd); + void execute_command_in_landing(vehicle_command_s cmd); + void execute_command_in_taking_off(vehicle_command_s cmd); + + void set_sub_mode(LOITER_SUB_MODE new_sub_mode); + + bool flag_sub_mode_goal_reached; }; #endif diff --git a/src/modules/navigator/loiter_params.c b/src/modules/navigator/loiter_params.c new file mode 100644 index 000000000..5c53c69d0 --- /dev/null +++ b/src/modules/navigator/loiter_params.c @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * 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 loter_params.c + * + * Parameter for loter. + * + * @author Martins Frolovs <martins.f@airdog.com> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/** + * Step length for one movement command + * + * @unit meters + * @min 1 + * @group Navigator + */ +PARAM_DEFINE_FLOAT(LOI_STEP_LEN, 5.00f); + + +/** + * Minimum altitude above target + * + * @unit meters + * @group Navigator + */ +PARAM_DEFINE_FLOAT(LOI_MIN_ALT, 2.00f); + + diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c44d4c35e..429f7cd22 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,10 +40,12 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ navigator_mode.cpp \ + navigator_mode_params.c \ mission_block.cpp \ mission.cpp \ mission_params.c \ loiter.cpp \ + loiter_params.c \ rtl.cpp \ rtl_params.c \ mission_feasibility_checker.cpp \ @@ -55,7 +57,9 @@ SRCS = navigator_main.cpp \ rcloss_params.c \ enginefailure.cpp \ gpsfailure.cpp \ - gpsfailure_params.c + gpsfailure_params.c \ + abs_follow.cpp \ + abs_follow_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d550dcc4c..f1c2d7caf 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -55,6 +55,10 @@ #include <uORB/topics/parameter_update.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/target_global_position.h> +#include <uORB/topics/commander_request.h> + +#include <commander/px4_custom_mode.h> #include "navigator_mode.h" #include "mission.h" @@ -65,9 +69,11 @@ #include "gpsfailure.h" #include "rcloss.h" #include "geofence.h" +#include "abs_follow.h" /** * Number of navigation modes that need on_active/on_inactive calls + * Currently: mission, loiter, rtl, offboard, abs_follow */ #define NAVIGATOR_MODE_ARRAY_SIZE 7 @@ -122,6 +128,7 @@ public: */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } + void set_commander_request_updated() { _commander_request_updated = true; } /** * Getters @@ -136,8 +143,11 @@ public: struct mission_result_s* get_mission_result() { return &_mission_result; } struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } + struct commander_request_s* get_commander_request() { return &_commander_request;}; + struct target_global_position_s* get_target_position() {return &_target_pos; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } + int get_vehicle_command_sub() { return _vcommand_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(); } @@ -161,12 +171,16 @@ private: int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ int _param_update_sub; /**< param update subscription */ + int _vcommand_sub; /**< vehicle control subscription */ + int _target_pos_sub; /**< target position subscription */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; orb_advert_t _att_sp_pub; /**< publish att sp used only in very special failsafe modes when pos control is deactivated */ + orb_advert_t _commander_request_pub; /**< publish commander requests */ vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -180,6 +194,8 @@ private: mission_result_s _mission_result; vehicle_attitude_setpoint_s _att_sp; + target_global_position_s _target_pos; /**< global target position */ + commander_request_s _commander_request; bool _mission_item_valid; /**< flags if the current mission item is valid */ @@ -200,11 +216,13 @@ private: EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ + AbsFollow _abs_follow; /**< class that handles AFollow */ 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 _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ + bool _commander_request_updated; /**< flags if commander request needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ @@ -249,6 +267,11 @@ private: * Update parameters */ void params_update(); + + /** + * Retrieve target global position + */ + void target_position_update(); /** * Shim for calling task_main from task_create. @@ -270,6 +293,11 @@ private: */ void publish_position_setpoint_triplet(); + /** + * Publish requests for commander + */ + void publish_commander_request(); + /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index def2af424..7fc4d3a1d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -108,9 +108,12 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _offboard_mission_sub(-1), _param_update_sub(-1), + _vcommand_sub(-1), + _target_pos_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), _att_sp_pub(-1), + _commander_request_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -135,8 +138,10 @@ Navigator::Navigator() : _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), + _abs_follow(this, "FOL"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), + _commander_request_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -150,6 +155,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_engineFailure; _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; + _navigation_mode_array[4] = &_abs_follow; updateParams(); } @@ -236,6 +242,12 @@ Navigator::params_update() } void +Navigator::target_position_update() +{ + orb_copy(ORB_ID(target_global_position), _target_pos_sub, &_target_pos); +} + +void Navigator::task_main_trampoline(int argc, char *argv[]) { navigator::g_navigator->task_main(); @@ -278,6 +290,8 @@ Navigator::task_main() _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)); + _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + _target_pos_sub = orb_subscribe(ORB_ID(target_global_position)); /* copy all topics first time */ vehicle_status_update(); @@ -288,6 +302,7 @@ Navigator::task_main() home_position_update(); navigation_capabilities_update(); params_update(); + target_position_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -296,7 +311,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + struct pollfd fds[9]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -315,6 +330,8 @@ Navigator::task_main() fds[6].events = POLLIN; fds[7].fd = _gps_pos_sub; fds[7].events = POLLIN; + fds[8].fd = _target_pos_sub; + fds[8].events = POLLIN; while (!_task_should_exit) { @@ -339,6 +356,11 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* target position updated */ + if (fds[8].revents & POLLIN) { + target_position_update(); + } + static bool have_geofence_position_data = false; /* gps updated */ @@ -354,10 +376,13 @@ Navigator::task_main() sensor_combined_update(); } + bool parameters_updated = false; + /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); updateParams(); + parameters_updated = true; } /* vehicle control mode updated */ @@ -454,6 +479,9 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LANDENGFAIL: _navigation_mode = &_engineFailure; break; + case NAVIGATION_STATE_AUTO_ABS_FOLLOW: + _navigation_mode = &_abs_follow; /* TODO: change this to something else */ + break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: _navigation_mode = &_gpsFailure; break; @@ -465,7 +493,7 @@ Navigator::task_main() /* iterate through navigation modes and set active/inactive for each */ for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { - _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); + _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i], parameters_updated); } /* if nothing is running, set position setpoint triplet invalid */ @@ -482,6 +510,11 @@ Navigator::task_main() _pos_sp_triplet_updated = false; } + if (_commander_request_updated) { + publish_commander_request(); + _commander_request_updated = false; + } + perf_end(_loop_perf); } warnx("exiting."); @@ -553,6 +586,18 @@ Navigator::publish_position_setpoint_triplet() } } +void +Navigator::publish_commander_request() +{ + /* publish commander request only once available */ + if (_commander_request_pub > 0) { + orb_publish(ORB_ID(commander_request), _commander_request_pub, &_commander_request); + + } else { + _commander_request_pub = orb_advertise(ORB_ID(commander_request), &_commander_request); + } +} + void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); @@ -563,7 +608,6 @@ void Navigator::load_fence_from_file(const char *filename) _geofence.loadFromFile(filename); } - static void usage() { errx(1, "usage: navigator {start|stop|status|fence|fencefile}"); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3807c5ea8..b86e03894 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -37,20 +37,53 @@ * * @author Julian Oes <julian@oes.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Martins Frolovs <martins.f@airdog.com> */ #include "navigator_mode.h" #include "navigator.h" +#include <uORB/uORB.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/parameter_update.h> + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#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 <systemlib/err.h> +#include <systemlib/systemlib.h> +#include <geo/geo.h> +#include <dataman/dataman.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> + + NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : SuperBlock(navigator, name), _navigator(navigator), _first_run(true) { - /* load initial params */ updateParams(); - /* set initial mission items */ on_inactive(); + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } NavigatorMode::~NavigatorMode() @@ -58,7 +91,60 @@ NavigatorMode::~NavigatorMode() } void -NavigatorMode::run(bool active) { +NavigatorMode::updateParameters() { + + updateParamHandles(); + updateParamValues(); + +} + +void +NavigatorMode::updateParamHandles() { + + _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + _parameter_handles.takeoff_acceptance_radius = param_find("NAV_TAKEOFF_ACR"); + _parameter_handles.acceptance_radius = param_find("NAV_ACC_RAD"); + _parameter_handles.velocity_lpf = param_find("NAV_VEL_LPF"); + + _parameter_handles.afol_use_cam_pitch = param_find("AFOL_USE_CAM_P"); + _parameter_handles.afol_rep_target_alt = param_find("AFOL_REP_TALT"); + + _parameter_handles.loi_min_alt = param_find("LOI_MIN_ALT"); + _parameter_handles.loi_step_len = param_find("LOI_STEP_LEN"); + + _parameter_handles.rtl_ret_alt = param_find("RTL_RET_ALT"); +} + +void +NavigatorMode::updateParamValues() { + + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); + param_get(_parameter_handles.takeoff_acceptance_radius, &(_parameters.takeoff_acceptance_radius)); + param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); + param_get(_parameter_handles.velocity_lpf, &(_parameters.velocity_lpf)); + + param_get(_parameter_handles.afol_use_cam_pitch, &(_parameters.afol_use_cam_pitch)); + param_get(_parameter_handles.afol_rep_target_alt, &(_parameters.afol_rep_target_alt)); + + param_get(_parameter_handles.loi_min_alt, &(_parameters.loi_min_alt)); + param_get(_parameter_handles.loi_step_len, &(_parameters.loi_step_len)); + + param_get(_parameter_handles.rtl_ret_alt, &(_parameters.rtl_ret_alt)); + + int toa = (int)_parameters.takeoff_alt; + mavlink_log_info(_navigator->get_mavlink_fd(), "Takeoff alt now is %d", toa); + +} + + +void +NavigatorMode::run(bool active, bool parameters_updated) { + + + if (parameters_updated) { + updateParameters(); + } + if (active) { if (_first_run) { /* first run */ @@ -96,3 +182,102 @@ void NavigatorMode::on_active() { } + +bool +NavigatorMode::update_vehicle_command() +{ + bool vcommand_updated = false; + orb_check(_navigator->get_vehicle_command_sub(), &vcommand_updated); + + if (vcommand_updated) { + + if (orb_copy(ORB_ID(vehicle_command), _navigator->get_vehicle_command_sub(), &_vcommand) == OK) { + return true; + } + else + return false; + } + + return false; +} + +void +NavigatorMode::execute_vehicle_command() +{ +} + +void +NavigatorMode::point_camera_to_target(position_setpoint_s *sp) +{ + target_pos = _navigator->get_target_position(); + global_pos = _navigator->get_global_position(); + + // Calculate offset values for later use. + float offset_x; + float offset_y; + float offset_z = target_pos->alt - global_pos->alt; + + get_vector_to_next_waypoint( + target_pos->lat, + target_pos->lon, + global_pos->lat, + global_pos->lon, + &offset_x, + &offset_y + ); + + math::Vector<3> offset(offset_x, offset_y, offset_z); + math::Vector<2> offset_xy(offset_x, offset_y); + + float offset_xy_len = offset_xy.length(); + + if (offset_xy_len > 2.0f) + sp->yaw = _wrap_pi(atan2f(-offset_xy(1), -offset_xy(0))); + + sp->camera_pitch = atan2f(offset(2), offset_xy_len); +} + +bool +NavigatorMode::check_current_pos_sp_reached() +{ + struct vehicle_status_s *vstatus = _navigator->get_vstatus(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + struct vehicle_global_position_s *global_pos = _navigator->get_global_position();; + + switch (pos_sp_triplet->current.type) + { + case SETPOINT_TYPE_IDLE: + return true; + break; + + case SETPOINT_TYPE_LAND: + return vstatus->condition_landed; + break; + + case SETPOINT_TYPE_TAKEOFF: + { + float alt_diff = fabs(pos_sp_triplet->current.alt - global_pos->alt); + return _parameters.takeoff_acceptance_radius >= alt_diff; + break; + } + case SETPOINT_TYPE_POSITION: + { + float dist_xy = -1; + float dist_z = -1; + + float distance = get_distance_to_point_global_wgs84( + global_pos->lat, global_pos->lon, global_pos->alt, + pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, pos_sp_triplet->current.alt, + &dist_xy, &dist_z + ); + + return _parameters.acceptance_radius >= distance; + + break; + } + default: + return false; + break; + + } +} diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index de5545dcb..c9fa738db 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -50,6 +50,7 @@ #include <dataman/dataman.h> #include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/vehicle_command.h> class Navigator; @@ -66,7 +67,7 @@ public: */ virtual ~NavigatorMode(); - void run(bool active); + void run(bool active, bool parameters_updated); /** * This function is called while the mode is inactive @@ -83,17 +84,88 @@ public: */ virtual void on_active(); + /* + * This function defines how vehicle reacts on commands in + * current navigator mode + */ + virtual void execute_vehicle_command(); + + /* + * Check if vehicle command subscription has been updated + * if it has it updates _vcommand structure and returns true + */ + bool update_vehicle_command(); + + void point_camera_to_target(position_setpoint_s *sp); + + void updateParameters(); + void updateParamValues(); + void updateParamHandles(); + + struct { + float takeoff_alt; + float takeoff_acceptance_radius; + float acceptance_radius; + float loiter_step; + float velocity_lpf; + + int afol_rep_target_alt; + int afol_use_cam_pitch; + + float loi_step_len; + float loi_min_alt; + + float rtl_ret_alt; + + } _parameters; + + + struct { + param_t takeoff_alt; + param_t takeoff_acceptance_radius; + param_t acceptance_radius; + param_t velocity_lpf; + + param_t afol_rep_target_alt; + param_t afol_use_cam_pitch; + + param_t loi_step_len; + param_t loi_min_alt; + + param_t rtl_ret_alt; + + } _parameter_handles; + + + protected: Navigator *_navigator; + struct position_setpoint_triplet_s *pos_sp_triplet; + + struct target_global_position_s *target_pos; + struct vehicle_global_position_s *global_pos; + struct home_position_s *home_pos; + + struct vehicle_command_s _vcommand; + + int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ + + bool check_current_pos_sp_reached(); + + + private: + bool _first_run; - /* this class has ptr data members, so it should not be copied, + /* + * This class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ NavigatorMode(const NavigatorMode&); NavigatorMode operator=(const NavigatorMode&); + }; #endif diff --git a/src/modules/navigator/navigator_mode_params.c b/src/modules/navigator/navigator_mode_params.c new file mode 100644 index 000000000..46e1a22ce --- /dev/null +++ b/src/modules/navigator/navigator_mode_params.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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_params.c + * + * Parameter for all navigator modes. + * + * @author Martins Frolovs <martins.f@airdog.com> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/* + * Navigator mode parameters + */ + +/** + * Take-off altitude + * + * Altitude to which vehicle will take-off. + * + * @unit meters + * @min 0 + * @group Navigator + */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); + + +/** + * Step size for one commend in loiter navigator mode + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 1 + * @group Navigator + */ +PARAM_DEFINE_FLOAT(LOI_STEP_LEN, 5.00f); + +/** + * Velocity LPF. + * + * Low pass filter. Time in seconds to average target movements. + * + * @unit seconds + * @group Navigator + */ +PARAM_DEFINE_FLOAT(NAV_VEL_LPF, 0.3f); + +/** + * Acceptance radius to determine if setpoint have been reached + * + * @unit meters + * @min 1 + * @max 50 + * @group Navigator + */ +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 2.00f); + +/** + * Minimum altitude above target in loiter. + * + * @unit meters + * @group Navigator + */ +PARAM_DEFINE_FLOAT(LOI_MIN_ALT, 2.00f); + +/** + * Acceptance radius for takeoff setpoint + * + * @unit meters + * @group Navigator + */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ACR, 2.00f); + + + diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1f40e634e..025fcbb0d 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,16 +55,6 @@ */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); -/** - * Acceptance Radius - * - * Default acceptance radius, overridden by acceptance radius of waypoint if set. - * - * @unit meters - * @min 1.0 - * @group Mission - */ -PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); /** * Set OBC mode for data link loss diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b6c4b8fdf..1e7689200 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -35,6 +35,7 @@ * Helper class to access RTL * @author Julian Oes <julian@oes.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Martins Frolovs <martins.f@airdog.com> */ #include <string.h> @@ -49,23 +50,17 @@ #include <uORB/uORB.h> #include <uORB/topics/mission.h> #include <uORB/topics/home_position.h> +#include <uORB/topics/position_setpoint_triplet.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, "RTL_RETURN_ALT", false), - _param_descend_alt(this, "RTL_DESCEND_ALT", false), - _param_land_delay(this, "RTL_LAND_DELAY", false) + MissionBlock(navigator, name) { - /* load initial params */ - updateParams(); - /* initial reset */ - on_inactive(); + updateParameters(); + rtl_state = RTL_STATE_NONE; } RTL::~RTL() @@ -75,243 +70,186 @@ RTL::~RTL() void RTL::on_inactive() { - /* reset RTL state only if setpoint moved */ - if (!_navigator->get_can_loiter_at_sp()) { - _rtl_state = RTL_STATE_NONE; - } } void RTL::on_activation() { - /* 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; - } + updateParameters(); + + global_pos = _navigator->get_global_position(); + home_pos = _navigator->get_home_position(); + pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + first_rtl_setpoint_set = false; + + float accaptance_radius = _parameters.acceptance_radius; + + float xy_distance = get_distance_to_next_waypoint( + global_pos->lat, global_pos->lon, + home_pos->lat, home_pos->lon + ); + + + /* Determine and set current RTL state */ + + /* Vehicle have already landed */ + if (_navigator->get_vstatus()->condition_landed) { + rtl_state = RTL_STATE_LANDED; + /* Already home we only need to land */ + } else if (xy_distance <= accaptance_radius) { + rtl_state = RTL_STATE_LAND; + /* No need climb */ + } else if ( global_pos->alt >= home_pos->alt + _parameters.rtl_ret_alt) { + rtl_state = RTL_STATE_RETURN; + } else { + rtl_state = RTL_STATE_CLIMB; } - set_rtl_item(); + set_rtl_setpoint(); + } void RTL::on_active() { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { - advance_rtl(); - set_rtl_item(); + + mv_fd = _navigator->get_mavlink_fd(); + global_pos = _navigator->get_global_position(); + home_pos = _navigator->get_home_position(); + pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + if ( update_vehicle_command() ) + execute_vehicle_command(); + + if (rtl_state != RTL_STATE_LANDED ) { + + if (!first_rtl_setpoint_set) { + set_rtl_setpoint(); + first_rtl_setpoint_set = true; + } else if (check_current_pos_sp_reached()) { + set_next_rtl_state(); + set_rtl_setpoint(); + } } + } void -RTL::set_rtl_item() +RTL::execute_vehicle_command() { - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* make sure we have the latest params */ - updateParams(); - - set_previous_pos_setpoint(); - _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; - } + // Update _parameter values with the latest navigator_mode parameters - 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; - } + vehicle_command_s cmd = _vcommand; - 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; + if (cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD) { + int remote_cmd = cmd.param1; + if (remote_cmd == REMOTE_CMD_PLAY_PAUSE) { + commander_request_s *commander_request = _navigator->get_commander_request(); + commander_request->request_type = V_MAIN_STATE_CHANGE; + commander_request->main_state = MAIN_STATE_AUTO_LOITER; + _navigator->set_commander_request_updated(); + } } - 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"); +} + +void +RTL::set_rtl_setpoint() +{ + switch (rtl_state) { + case RTL_STATE_CLIMB: { + float climb_alt = _navigator->get_home_position()->alt + _parameters.rtl_ret_alt; + + pos_sp_triplet->current.lat = global_pos->lat; + pos_sp_triplet->current.lon = global_pos->lon; + pos_sp_triplet->current.alt = climb_alt; + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + break; } - break; - } + case RTL_STATE_RETURN: { - 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; - } + global_pos = _navigator->get_global_position(); + home_pos = _navigator->get_home_position(); - 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; - } + // Calculate offset values for later use. + float offset_x; + float offset_y; + float offset_z = target_pos->alt - global_pos->alt; - default: - break; - } + get_vector_to_next_waypoint( + global_pos->lat, + global_pos->lon, + home_pos->lat, + home_pos->lon, + &offset_x, + &offset_y + ); - reset_mission_item_reached(); + math::Vector<3> offset(offset_x, offset_y, offset_z); + math::Vector<2> offset_xy(offset_x, offset_y); - /* convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; + float offset_xy_len = offset_xy.length(); + + if (offset_xy_len > 2.0f) + pos_sp_triplet->current.yaw = _wrap_pi(atan2f(-offset_xy(1), -offset_xy(0))); + + pos_sp_triplet->current.lat = home_pos->lat; + pos_sp_triplet->current.lon = home_pos->lon; + pos_sp_triplet->current.alt = global_pos->alt; + pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION; + + break; + } + case RTL_STATE_LAND: { + pos_sp_triplet->current.lat = home_pos->lat; + pos_sp_triplet->current.lon = home_pos->lon; + pos_sp_triplet->current.alt = global_pos->alt; + pos_sp_triplet->current.type = SETPOINT_TYPE_LAND; + + break; + } + case RTL_STATE_LANDED: { + + disarm(); + + break; + } + default: + break; + } _navigator->set_position_setpoint_triplet_updated(); + } void -RTL::advance_rtl() +RTL::set_next_rtl_state() { - 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; + switch (rtl_state) { + case RTL_STATE_CLIMB: + rtl_state = RTL_STATE_RETURN; + break; - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - break; + case RTL_STATE_RETURN: + rtl_state = RTL_STATE_LAND; + break; - default: - break; + case RTL_STATE_LAND: + rtl_state = RTL_STATE_LANDED; + break; + + default: + break; } } + +void +RTL::disarm() +{ + commander_request_s *commander_request = _navigator->get_commander_request(); + commander_request->request_type = V_DISARM; + _navigator->set_commander_request_updated(); +} diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 5928f1f07..84ab155dc 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,6 +44,8 @@ #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> +#include <uORB/uORB.h> + #include <uORB/topics/mission.h> #include <uORB/topics/mission.h> #include <uORB/topics/home_position.h> @@ -52,6 +54,7 @@ #include "navigator_mode.h" #include "mission_block.h" + class Navigator; class RTL : public MissionBlock @@ -67,30 +70,28 @@ public: virtual void on_active(); + virtual void execute_vehicle_command(); + private: - /** - * Set the RTL item - */ - void set_rtl_item(); - /** - * Move to next RTL item - */ - void advance_rtl(); + void set_rtl_setpoint(); - enum RTLState { + void set_next_rtl_state(); + + void disarm(); + + enum RTL_state { RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, RTL_STATE_LAND, RTL_STATE_LANDED, - } _rtl_state; + } rtl_state; + + float return_alt; + int mv_fd; + bool first_rtl_setpoint_set; - 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 index bfe6ce7e1..6a54aee95 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -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,65 +34,21 @@ /** * @file rtl_params.c * - * Parameters for RTL + * Parameter for RTL. * - * @author Julian Oes <julian@oes.ch> + * @author Martins Frolovs <martins.f@airdog.com> */ #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 + * @min 1 + * @group Navigator */ -PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); +PARAM_DEFINE_FLOAT(RTL_RET_ALT, 30.00f); |