Line data    Source code

       1             : #include "AP_Frsky_MAVliteMsgHandler.h"
       2             : 
       3             : #include <AC_Fence/AC_Fence.h>
       4             : #include <AP_Vehicle/AP_Vehicle.h>
       5             : #include <GCS_MAVLink/GCS.h>
       6             : 
       7             : extern const AP_HAL::HAL& hal;
       8             : 
       9             : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
      10             : /*
      11             :  * Handle the COMMAND_LONG mavlite message
      12             :  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
      13             :  */
      14             : 
      15           4 : void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg)
      16             : {
      17           4 :     mavlink_command_long_t mav_command_long {};
      18             : 
      19             :     uint8_t cmd_options;
      20           4 :     float params[7] {};
      21             : 
      22           4 :     if (!rxmsg.get_uint16(mav_command_long.command, 0)) {
      23           0 :         return;
      24             :     }
      25           4 :     if (!rxmsg.get_uint8(cmd_options, 2)) {
      26           0 :         return;
      27             :     }
      28           4 :     uint8_t param_count = AP_Frsky_MAVlite_Message::bit8_unpack(cmd_options, 3, 0);               // first 3 bits
      29             : 
      30          10 :     for (uint8_t cmd_idx=0; cmd_idx<param_count; cmd_idx++) {
      31             :         // base offset is 3, relative offset is 4*cmd_idx
      32           6 :         if (!rxmsg.get_float(params[cmd_idx], 3+(4*cmd_idx))) {
      33           0 :             return;
      34             :         }
      35             :     }
      36             : 
      37           4 :     mav_command_long.param1 = params[0];
      38           4 :     mav_command_long.param2 = params[1];
      39           4 :     mav_command_long.param3 = params[2];
      40           4 :     mav_command_long.param4 = params[3];
      41           4 :     mav_command_long.param5 = params[4];
      42           4 :     mav_command_long.param6 = params[5];
      43           4 :     mav_command_long.param7 = params[6];
      44             : 
      45           4 :     const MAV_RESULT mav_result = handle_command(mav_command_long);
      46           4 :     send_command_ack(mav_result, mav_command_long.command);
      47             : }
      48             : 
      49           4 : MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command(const mavlink_command_long_t &mav_command_long)
      50             : {
      51             :     // filter allowed commands
      52           4 :     switch (mav_command_long.command) {
      53             :         //case MAV_CMD_ACCELCAL_VEHICLE_POS:
      54           2 :         case MAV_CMD_DO_SET_MODE:
      55           2 :             return handle_command_do_set_mode(mav_command_long);
      56             :         //case MAV_CMD_DO_SET_HOME:
      57             : #if AP_FENCE_ENABLED
      58           1 :         case MAV_CMD_DO_FENCE_ENABLE:
      59           1 :             return handle_command_do_fence_enable(mav_command_long);
      60             : #endif
      61           0 :         case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
      62           0 :             return handle_command_preflight_reboot(mav_command_long);
      63             :         //case MAV_CMD_DO_START_MAG_CAL:
      64             :         //case MAV_CMD_DO_ACCEPT_MAG_CAL:
      65             :         //case MAV_CMD_DO_CANCEL_MAG_CAL:
      66             :         //case MAV_CMD_START_RX_PAIR:
      67             :         //case MAV_CMD_DO_DIGICAM_CONFIGURE:
      68             :         //case MAV_CMD_DO_DIGICAM_CONTROL:
      69             :         //case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
      70             :         //case MAV_CMD_DO_GRIPPER:
      71             :         //case MAV_CMD_DO_MOUNT_CONFIGURE:
      72             :         //case MAV_CMD_DO_MOUNT_CONTROL:
      73             :         //case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
      74             :         //case MAV_CMD_DO_SET_ROI_SYSID:
      75             :         //case MAV_CMD_DO_SET_ROI_LOCATION:
      76             :         //case MAV_CMD_DO_SET_ROI:
      77           1 :         case MAV_CMD_PREFLIGHT_CALIBRATION:
      78           1 :             return handle_command_preflight_calibration_baro(mav_command_long);
      79             :         //case MAV_CMD_BATTERY_RESET:
      80             :         //case MAV_CMD_PREFLIGHT_UAVCAN:
      81             :         //case MAV_CMD_FLASH_BOOTLOADER:
      82             :         //case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
      83             :         //case MAV_CMD_GET_HOME_POSITION:
      84             :         //case MAV_CMD_PREFLIGHT_STORAGE:
      85             :         //case MAV_CMD_SET_MESSAGE_INTERVAL:
      86             :         //case MAV_CMD_GET_MESSAGE_INTERVAL:
      87             :         //case MAV_CMD_REQUEST_MESSAGE:
      88             :         //case MAV_CMD_DO_SET_SERVO:
      89             :         //case MAV_CMD_DO_REPEAT_SERVO:
      90             :         //case MAV_CMD_DO_SET_RELAY:
      91             :         //case MAV_CMD_DO_REPEAT_RELAY:
      92             :         //case MAV_CMD_DO_FLIGHTTERMINATION:
      93             :         //case MAV_CMD_COMPONENT_ARM_DISARM:
      94             :         //case MAV_CMD_FIXED_MAG_CAL_YAW:
      95           0 :         default:
      96           0 :             return MAV_RESULT_UNSUPPORTED;
      97             :     }
      98             :     return MAV_RESULT_UNSUPPORTED;
      99             : }
     100             : 
     101           2 : MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_set_mode(const mavlink_command_long_t &mav_command_long)
     102             : {
     103           2 :     if (AP::vehicle()->set_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) {
     104           2 :         return MAV_RESULT_ACCEPTED;
     105             :     }
     106           0 :     return MAV_RESULT_FAILED;
     107             : }
     108             : 
     109           4 : void AP_Frsky_MAVliteMsgHandler::send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid)
     110             : {
     111           4 :     AP_Frsky_MAVlite_Message txmsg;
     112           4 :     txmsg.msgid = MAVLINK_MSG_ID_COMMAND_ACK;
     113           4 :     if (!txmsg.set_uint16(cmdid, 0)) {
     114           0 :         return;
     115             :     }
     116           4 :     if (!txmsg.set_uint8((uint8_t)mav_result, 2)) {
     117           0 :         return;
     118             :     }
     119           4 :     send_message(txmsg);
     120             : }
     121             : 
     122           1 : MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_calibration_baro(const mavlink_command_long_t &mav_command_long)
     123             : {
     124           1 :     if (!(is_equal(mav_command_long.param1,0.0f) && is_equal(mav_command_long.param2,0.0f) && is_equal(mav_command_long.param3,1.0f))) {
     125           0 :         return MAV_RESULT_FAILED;
     126             :     }
     127             : 
     128           1 :     if (hal.util->get_soft_armed()) {
     129           0 :         return MAV_RESULT_DENIED;
     130             :     }
     131             :     // fast barometer calibration
     132           1 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Updating barometer calibration");
     133           1 :     AP::baro().update_calibration();
     134           1 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer calibration complete");
     135             : 
     136             : #if AP_AIRSPEED_ENABLED
     137           1 :     AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
     138           1 :     if (airspeed != nullptr) {
     139           1 :         airspeed->calibrate(false);
     140             :     }
     141             : #endif
     142             : 
     143           1 :     return MAV_RESULT_ACCEPTED;
     144             : }
     145             : 
     146             : #if AP_FENCE_ENABLED
     147           1 : MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(const mavlink_command_long_t &mav_command_long)
     148             : {
     149           1 :     AC_Fence *fence = AP::fence();
     150           1 :     if (fence == nullptr) {
     151           0 :         return MAV_RESULT_UNSUPPORTED;
     152             :     }
     153             : 
     154           1 :     switch ((uint16_t)mav_command_long.param1) {
     155           0 :         case 0:
     156           0 :             fence->enable_configured(false);
     157           0 :             return MAV_RESULT_ACCEPTED;
     158           1 :         case 1:
     159           1 :             fence->enable_configured(true);
     160           1 :             return MAV_RESULT_ACCEPTED;
     161           0 :         default:
     162           0 :             return MAV_RESULT_FAILED;
     163             :     }
     164             : }
     165             : #endif // AP_FENCE_ENABLED
     166             : 
     167             : /*
     168             :  * Handle the PARAM_REQUEST_READ mavlite message
     169             :  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
     170             :  */
     171           1 : void AP_Frsky_MAVliteMsgHandler::handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg)
     172             : {
     173             :     float param_value;
     174             :     char param_name[AP_MAX_NAME_SIZE+1];
     175           1 :     if (!rxmsg.get_string(param_name, 0)) {
     176           0 :         return;
     177             :     }
     178             :     // find existing param
     179           1 :     if (!AP_Param::get(param_name,param_value)) {
     180           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);
     181           0 :         return;
     182             :     }
     183           1 :     AP_Frsky_MAVlite_Message txmsg;
     184           1 :     txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
     185           1 :     if (!txmsg.set_float(param_value, 0)) {
     186           0 :         return;
     187             :     }
     188           1 :     if (!txmsg.set_string(param_name, 4)) {
     189           0 :         return;
     190             :     }
     191           1 :     send_message(txmsg);
     192             : }
     193             : 
     194             : /*
     195             :  * Handle the PARAM_SET mavlite message
     196             :  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
     197             :  */
     198           1 : void AP_Frsky_MAVliteMsgHandler::handle_param_set(const AP_Frsky_MAVlite_Message &rxmsg)
     199             : {
     200             :     float param_value;
     201             :     char param_name[AP_MAX_NAME_SIZE+1];
     202             :     // populate packet with mavlite payload
     203           1 :     if (!rxmsg.get_float(param_value, 0)) {
     204           0 :         return;
     205             :     }
     206           1 :     if (!rxmsg.get_string(param_name, 4)) {
     207           0 :         return;
     208             :     }
     209             :     // find existing param so we can get the old value
     210             :     enum ap_var_type var_type;
     211             :     // set parameter
     212             :     AP_Param *vp;
     213           1 :     uint16_t parameter_flags = 0;
     214           1 :     vp = AP_Param::find(param_name, &var_type, &parameter_flags);
     215           1 :     if (vp == nullptr || isnan(param_value) || isinf(param_value)) {
     216           0 :         return;
     217             :     }
     218           1 :     if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) {
     219             :         // the user can set BRD_OPTIONS to enable set of internal
     220             :         // parameters, for developer testing or unusual use cases
     221           0 :         if (AP_BoardConfig::allow_set_internal_parameters()) {
     222           0 :             parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY;
     223             :         }
     224             :     }
     225           1 :     if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
     226           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param write denied (%s)", param_name);
     227           1 :     } else if (!AP_Param::set_and_save_by_name(param_name, param_value)) {
     228           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param write failed (%s)", param_name);
     229             :     }
     230             :     // let's read back the last value, either the readonly one or the updated one
     231           1 :     if (!AP_Param::get(param_name,param_value)) {
     232           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);
     233           0 :         return;
     234             :     }
     235           1 :     AP_Frsky_MAVlite_Message txmsg;
     236           1 :     txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
     237           1 :     if (!txmsg.set_float(param_value, 0)) {
     238           0 :         return;
     239             :     }
     240           1 :     if (!txmsg.set_string(param_name, 4)) {
     241           0 :         return;
     242             :     }
     243           1 :     send_message(txmsg);
     244             : }
     245             : 
     246             : /*
     247             :   Handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command 
     248             :   for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
     249             :   Optionally disable PX4IO overrides. This is done for quadplanes to
     250             :   prevent the mixer running while rebooting which can start the VTOL
     251             :   motors. That can be dangerous when a preflight reboot is done with
     252             :   the pilot close to the aircraft and can also damage the aircraft
     253             :  */
     254           0 : MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_reboot(const mavlink_command_long_t &mav_command_long)
     255             : {
     256           0 :     if (!(is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2))) {
     257           0 :         return MAV_RESULT_FAILED;
     258             :     }
     259           0 :     if (hal.util->get_soft_armed()) {
     260             :         // refuse reboot when armed
     261           0 :         return MAV_RESULT_FAILED;
     262             :     }
     263             : 
     264           0 :     AP::vehicle()->reboot(false);
     265             : 
     266           0 :     return MAV_RESULT_FAILED;
     267             : }
     268             : 
     269             : /*
     270             :  * Process an incoming mavlite message
     271             :  * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
     272             :  */
     273           6 : void AP_Frsky_MAVliteMsgHandler::process_message(const AP_Frsky_MAVlite_Message &rxmsg)
     274             : {
     275           6 :     switch (rxmsg.msgid) {
     276           1 :         case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
     277           1 :             handle_param_request_read(rxmsg);
     278           1 :             break;
     279           1 :         case MAVLINK_MSG_ID_PARAM_SET:
     280           1 :             handle_param_set(rxmsg);
     281           1 :             break;
     282           4 :         case MAVLINK_MSG_ID_COMMAND_LONG:
     283           4 :             handle_command_long(rxmsg);
     284           4 :             break;
     285             :     }
     286           6 : }
     287             : 
     288             : /*
     289             :  * Send a mavlite message
     290             :  */
     291           6 : bool AP_Frsky_MAVliteMsgHandler::send_message(AP_Frsky_MAVlite_Message &txmsg)
     292             : {
     293           6 :     return _send_fn(txmsg);
     294             : }
     295             : #endif