Line data    Source code

       1             : /*
       2             :   Implementation details for transfering rally point information using
       3             :   the MISSION_ITEM protocol to and from a GCS.
       4             : 
       5             :   This program is free software: you can redistribute it and/or modify
       6             :   it under the terms of the GNU General Public License as published by
       7             :   the Free Software Foundation, either version 3 of the License, or
       8             :   (at your option) any later version.
       9             : 
      10             :   This program is distributed in the hope that it will be useful,
      11             :   but WITHOUT ANY WARRANTY; without even the implied warranty of
      12             :   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      13             :   GNU General Public License for more details.
      14             : 
      15             :   You should have received a copy of the GNU General Public License
      16             :   along with this program.  If not, see <http://www.gnu.org/licenses/>.
      17             : */
      18             : 
      19             : #include "GCS_config.h"
      20             : #include <AP_Rally/AP_Rally_config.h>
      21             : 
      22             : #if HAL_GCS_ENABLED && HAL_RALLY_ENABLED
      23             : 
      24             : #include "MissionItemProtocol_Rally.h"
      25             : 
      26             : #include <AP_Logger/AP_Logger.h>
      27             : #include <AP_Rally/AP_Rally.h>
      28             : #include <GCS_MAVLink/GCS.h>
      29             : #include <AP_Terrain/AP_Terrain.h>
      30             : 
      31           6 : MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd)
      32             : {
      33             :     RallyLocation rallyloc;
      34           6 :     const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
      35           6 :     if (ret != MAV_MISSION_ACCEPTED) {
      36           0 :         return ret;
      37             :     }
      38           6 :     if (!rally.append(rallyloc)) {
      39           0 :         return MAV_MISSION_ERROR;
      40             :     }
      41           6 :     return MAV_MISSION_ACCEPTED;
      42             : }
      43             : 
      44         886 : MAV_MISSION_RESULT MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link)
      45             : {
      46             : #if HAL_LOGGING_ENABLED
      47         886 :     AP::logger().Write_Rally();
      48             : #endif
      49         886 :     return MAV_MISSION_ACCEPTED;
      50             : }
      51             : 
      52           0 : bool MissionItemProtocol_Rally::clear_all_items()
      53             : {
      54           0 :     rally.truncate(0);
      55           0 :     return true;
      56             : }
      57             : 
      58           8 : MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret)
      59             : {
      60           8 :     if (cmd.command != MAV_CMD_NAV_RALLY_POINT) {
      61           0 :         return MAV_MISSION_UNSUPPORTED;
      62             :     }
      63           8 :     if (!check_lat(cmd.x)) {
      64           0 :         return MAV_MISSION_INVALID_PARAM5_X;
      65             :     }
      66           8 :     if (!check_lng(cmd.y)) {
      67           0 :         return MAV_MISSION_INVALID_PARAM6_Y;
      68             :     }
      69           8 :     if (cmd.z < INT16_MIN || cmd.z > INT16_MAX) {
      70           0 :         return MAV_MISSION_INVALID_PARAM7;
      71             :     }
      72           8 :     ret = {};
      73             : 
      74           8 :     switch (cmd.frame) {
      75           2 :         case MAV_FRAME_GLOBAL:
      76             :         case MAV_FRAME_GLOBAL_INT:
      77           2 :             ret.alt_frame = uint8_t(Location::AltFrame::ABSOLUTE);
      78           2 :             break;
      79             : 
      80           6 :         case MAV_FRAME_GLOBAL_RELATIVE_ALT:
      81             :         case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
      82           6 :             ret.alt_frame = uint8_t(Location::AltFrame::ABOVE_HOME);
      83           6 :             break;
      84             : 
      85             : #if AP_TERRAIN_AVAILABLE
      86           0 :         case MAV_FRAME_GLOBAL_TERRAIN_ALT:
      87             :         case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
      88           0 :             ret.alt_frame = uint8_t(Location::AltFrame::ABOVE_TERRAIN);
      89           0 :             break;
      90             : #endif
      91             : 
      92           0 :         default:
      93           0 :             return MAV_MISSION_UNSUPPORTED_FRAME;
      94             :     }
      95             : 
      96             :     // Fresh points always use new alt frame format
      97           8 :     ret.alt_frame_valid = true;
      98             : 
      99           8 :     ret.lat = cmd.x;
     100           8 :     ret.lng = cmd.y;
     101           8 :     ret.alt = cmd.z;
     102           8 :     return MAV_MISSION_ACCEPTED;
     103             : }
     104             : 
     105             : /*
     106             :   static function to get rally item as mavlink_mission_item_int_t
     107             :  */
     108           5 : bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq,
     109             :                                                          mavlink_mission_item_int_t &ret_packet)
     110             : {
     111           5 :     auto *rallyp = AP::rally();
     112             : 
     113             :     RallyLocation rallypoint;
     114             : 
     115           5 :     if (rallyp == nullptr || !rallyp->get_rally_point_with_index(seq, rallypoint)) {
     116           0 :         return false;
     117             :     }
     118             : 
     119             :     // Default to relative to home
     120           5 :     ret_packet = {
     121           5 :         x: rallypoint.lat,
     122           5 :         y: rallypoint.lng,
     123           5 :         z: float(rallypoint.alt),
     124             :         seq: seq,
     125             :         command: MAV_CMD_NAV_RALLY_POINT,
     126             :         frame: MAV_FRAME_GLOBAL_RELATIVE_ALT,
     127             :         mission_type: MAV_MISSION_TYPE_RALLY,
     128             :     };
     129             : 
     130           5 :     if (rallypoint.alt_frame_valid == 1) {
     131           5 :         switch (Location::AltFrame(rallypoint.alt_frame)) {
     132           2 :             case Location::AltFrame::ABSOLUTE:
     133           2 :                 ret_packet.frame = MAV_FRAME_GLOBAL;
     134           2 :                 break;
     135             : 
     136           3 :             case Location::AltFrame::ABOVE_HOME:
     137           3 :                 ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
     138           3 :                 break;
     139             : 
     140           0 :             case Location::AltFrame::ABOVE_ORIGIN:
     141             :                 // Above origin alt frame is not supported
     142           0 :                 return false;
     143             : 
     144           0 :             case Location::AltFrame::ABOVE_TERRAIN:
     145           0 :                 ret_packet.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT;
     146           0 :                 break;
     147             :         }
     148             :     }
     149             : 
     150           5 :     return true;
     151             : }
     152             : 
     153           5 : MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet)
     154             : {
     155           5 :     if (seq >= item_count()) {
     156           0 :         return MAV_MISSION_INVALID_SEQUENCE;
     157             :     }
     158             : 
     159           5 :     if (!get_item_as_mission_item(seq, ret_packet)) {
     160           0 :         return MAV_MISSION_INVALID_SEQUENCE;
     161             :     }
     162           5 :     return MAV_MISSION_ACCEPTED;
     163             : }
     164             : 
     165          17 : uint16_t MissionItemProtocol_Rally::item_count() const {
     166          17 :     return rally.get_rally_total();
     167             : }
     168             : 
     169         886 : uint16_t MissionItemProtocol_Rally::max_items() const {
     170         886 :     return rally.get_rally_max();
     171             : }
     172             : 
     173           2 : MAV_MISSION_RESULT MissionItemProtocol_Rally::replace_item(const mavlink_mission_item_int_t &cmd)
     174             : {
     175             :     RallyLocation rallyloc;
     176           2 :     const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
     177           2 :     if (ret != MAV_MISSION_ACCEPTED) {
     178           0 :         return ret;
     179             :     }
     180           2 :     if (!rally.set_rally_point_with_index(cmd.seq, rallyloc)) {
     181           0 :         return MAV_MISSION_ERROR;
     182             :     }
     183           2 :     return MAV_MISSION_ACCEPTED;
     184             : }
     185             : 
     186           0 : void MissionItemProtocol_Rally::timeout()
     187             : {
     188           0 :     link->send_text(MAV_SEVERITY_WARNING, "Rally upload timeout");
     189           0 : }
     190             : 
     191         886 : void MissionItemProtocol_Rally::truncate(const mavlink_mission_count_t &packet)
     192             : {
     193         886 :     rally.truncate(packet.count);
     194         886 : }
     195             : 
     196             : #endif  // HAL_GCS_ENABLED && HAL_RALLY_ENABLED