Line data    Source code

       1             : /*
       2             :   Implementation details for transfering waypoint 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_Mission/AP_Mission_config.h>
      21             : 
      22             : #if HAL_GCS_ENABLED && AP_MISSION_ENABLED
      23             : 
      24             : #include "MissionItemProtocol_Waypoints.h"
      25             : 
      26             : #include <AP_Logger/AP_Logger.h>
      27             : #include <AP_Mission/AP_Mission.h>
      28             : 
      29             : #include "GCS.h"
      30             : 
      31        2775 : MAV_MISSION_RESULT MissionItemProtocol_Waypoints::append_item(const mavlink_mission_item_int_t &mission_item_int)
      32             : {
      33             :     // sanity check for DO_JUMP command
      34        2775 :     AP_Mission::Mission_Command cmd {};
      35             : 
      36        2775 :     const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
      37        2775 :     if (res != MAV_MISSION_ACCEPTED) {
      38           0 :         return res;
      39             :     }
      40             : 
      41        2775 :     if (cmd.id == MAV_CMD_DO_JUMP) {
      42         115 :         if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) {
      43           0 :             return MAV_MISSION_ERROR;
      44             :         }
      45             :     }
      46             : 
      47        2775 :     if (!mission.add_cmd(cmd)) {
      48           0 :         return MAV_MISSION_ERROR;
      49             :     }
      50        2775 :     return MAV_MISSION_ACCEPTED;
      51             : }
      52             : 
      53           3 : bool MissionItemProtocol_Waypoints::clear_all_items()
      54             : {
      55           3 :     return mission.clear();
      56             : }
      57             : 
      58        1287 : MAV_MISSION_RESULT MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link)
      59             : {
      60        1287 :     _link.send_text(MAV_SEVERITY_INFO, "Flight plan received");
      61             : #if HAL_LOGGING_ENABLED
      62        1287 :     AP::logger().Write_EntireMission();
      63             : #endif
      64        1287 :     return MAV_MISSION_ACCEPTED;
      65             : }
      66             : 
      67        4633 : MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet)
      68             : {
      69        8852 :     if (seq != 0 && // always allow HOME to be read
      70        4219 :         seq >= mission.num_commands()) {
      71           0 :         return MAV_MISSION_INVALID_SEQUENCE;
      72             :     }
      73             : 
      74        4633 :     AP_Mission::Mission_Command cmd;
      75             : 
      76             :     // retrieve mission from eeprom
      77        4633 :     if (!mission.read_cmd_from_storage(seq, cmd)) {
      78           0 :         return MAV_MISSION_ERROR;
      79             :     }
      80             : 
      81        4633 :     if (!AP_Mission::mission_cmd_to_mavlink_int(cmd, ret_packet)) {
      82           0 :         return MAV_MISSION_ERROR;
      83             :     }
      84        4633 :     ret_packet.mission_type = MAV_MISSION_TYPE_MISSION;
      85             : 
      86             :     // set packet's current field to 1 if this is the command being executed
      87        4633 :     if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) {
      88           1 :         ret_packet.current = 1;
      89             :     }
      90             : 
      91        4633 :     return MAV_MISSION_ACCEPTED;
      92             : }
      93             : 
      94        4827 : uint16_t MissionItemProtocol_Waypoints::item_count() const {
      95        4827 :     return mission.num_commands();
      96             : }
      97             : 
      98        1284 : uint16_t MissionItemProtocol_Waypoints::max_items() const {
      99        1284 :     return mission.num_commands_max();
     100             : }
     101             : 
     102        1498 : MAV_MISSION_RESULT MissionItemProtocol_Waypoints::replace_item(const mavlink_mission_item_int_t &mission_item_int)
     103             : {
     104        1498 :     AP_Mission::Mission_Command cmd {};
     105             : 
     106        1498 :     const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
     107        1498 :     if (res != MAV_MISSION_ACCEPTED) {
     108           0 :         return res;
     109             :     }
     110             : 
     111             :     // sanity check for DO_JUMP command
     112        1498 :     if (cmd.id == MAV_CMD_DO_JUMP) {
     113          15 :         if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) {
     114           0 :             return MAV_MISSION_ERROR;
     115             :         }
     116             :     }
     117        1498 :     if (!mission.replace_cmd(cmd.index, cmd)) {
     118           0 :         return MAV_MISSION_ERROR;
     119             :     }
     120        1498 :     return MAV_MISSION_ACCEPTED;
     121             : }
     122             : 
     123           0 : void MissionItemProtocol_Waypoints::timeout()
     124             : {
     125           0 :     link->send_text(MAV_SEVERITY_WARNING, "Mission upload timeout");
     126           0 : }
     127             : 
     128        1284 : void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &packet)
     129             : {
     130             :     // new mission arriving, truncate mission to be the same length
     131        1284 :     mission.truncate(packet.count);
     132        1284 : }
     133             : 
     134             : #endif  // HAL_GCS_ENABLED && AP_MISSION_ENABLED