/**************************************************************************** * * 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 * @author Anton Babushkin * @author Martins Frolovs */ #include #include #include #include #include #include #include #include #include #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; } } } }