mirror of https://github.com/ArduPilot/ardupilot
296 lines
9.5 KiB
C++
296 lines
9.5 KiB
C++
#include "AP_Frsky_MAVliteMsgHandler.h"
|
|
|
|
#include <AC_Fence/AC_Fence.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
/*
|
|
* Handle the COMMAND_LONG mavlite message
|
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
|
*/
|
|
|
|
void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg)
|
|
{
|
|
mavlink_command_long_t mav_command_long {};
|
|
|
|
uint8_t cmd_options;
|
|
float params[7] {};
|
|
|
|
if (!rxmsg.get_uint16(mav_command_long.command, 0)) {
|
|
return;
|
|
}
|
|
if (!rxmsg.get_uint8(cmd_options, 2)) {
|
|
return;
|
|
}
|
|
uint8_t param_count = AP_Frsky_MAVlite_Message::bit8_unpack(cmd_options, 3, 0); // first 3 bits
|
|
|
|
for (uint8_t cmd_idx=0; cmd_idx<param_count; cmd_idx++) {
|
|
// base offset is 3, relative offset is 4*cmd_idx
|
|
if (!rxmsg.get_float(params[cmd_idx], 3+(4*cmd_idx))) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
mav_command_long.param1 = params[0];
|
|
mav_command_long.param2 = params[1];
|
|
mav_command_long.param3 = params[2];
|
|
mav_command_long.param4 = params[3];
|
|
mav_command_long.param5 = params[4];
|
|
mav_command_long.param6 = params[5];
|
|
mav_command_long.param7 = params[6];
|
|
|
|
const MAV_RESULT mav_result = handle_command(mav_command_long);
|
|
send_command_ack(mav_result, mav_command_long.command);
|
|
}
|
|
|
|
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command(const mavlink_command_long_t &mav_command_long)
|
|
{
|
|
// filter allowed commands
|
|
switch (mav_command_long.command) {
|
|
//case MAV_CMD_ACCELCAL_VEHICLE_POS:
|
|
case MAV_CMD_DO_SET_MODE:
|
|
return handle_command_do_set_mode(mav_command_long);
|
|
//case MAV_CMD_DO_SET_HOME:
|
|
#if AP_FENCE_ENABLED
|
|
case MAV_CMD_DO_FENCE_ENABLE:
|
|
return handle_command_do_fence_enable(mav_command_long);
|
|
#endif
|
|
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
|
return handle_command_preflight_reboot(mav_command_long);
|
|
//case MAV_CMD_DO_START_MAG_CAL:
|
|
//case MAV_CMD_DO_ACCEPT_MAG_CAL:
|
|
//case MAV_CMD_DO_CANCEL_MAG_CAL:
|
|
//case MAV_CMD_START_RX_PAIR:
|
|
//case MAV_CMD_DO_DIGICAM_CONFIGURE:
|
|
//case MAV_CMD_DO_DIGICAM_CONTROL:
|
|
//case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
|
|
//case MAV_CMD_DO_GRIPPER:
|
|
//case MAV_CMD_DO_MOUNT_CONFIGURE:
|
|
//case MAV_CMD_DO_MOUNT_CONTROL:
|
|
//case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
|
|
//case MAV_CMD_DO_SET_ROI_SYSID:
|
|
//case MAV_CMD_DO_SET_ROI_LOCATION:
|
|
//case MAV_CMD_DO_SET_ROI:
|
|
case MAV_CMD_PREFLIGHT_CALIBRATION:
|
|
return handle_command_preflight_calibration_baro(mav_command_long);
|
|
//case MAV_CMD_BATTERY_RESET:
|
|
//case MAV_CMD_PREFLIGHT_UAVCAN:
|
|
//case MAV_CMD_FLASH_BOOTLOADER:
|
|
//case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
|
|
//case MAV_CMD_GET_HOME_POSITION:
|
|
//case MAV_CMD_PREFLIGHT_STORAGE:
|
|
//case MAV_CMD_SET_MESSAGE_INTERVAL:
|
|
//case MAV_CMD_GET_MESSAGE_INTERVAL:
|
|
//case MAV_CMD_REQUEST_MESSAGE:
|
|
//case MAV_CMD_DO_SET_SERVO:
|
|
//case MAV_CMD_DO_REPEAT_SERVO:
|
|
//case MAV_CMD_DO_SET_RELAY:
|
|
//case MAV_CMD_DO_REPEAT_RELAY:
|
|
//case MAV_CMD_DO_FLIGHTTERMINATION:
|
|
//case MAV_CMD_COMPONENT_ARM_DISARM:
|
|
//case MAV_CMD_FIXED_MAG_CAL_YAW:
|
|
default:
|
|
return MAV_RESULT_UNSUPPORTED;
|
|
}
|
|
return MAV_RESULT_UNSUPPORTED;
|
|
}
|
|
|
|
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_set_mode(const mavlink_command_long_t &mav_command_long)
|
|
{
|
|
if (AP::vehicle()->set_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) {
|
|
return MAV_RESULT_ACCEPTED;
|
|
}
|
|
return MAV_RESULT_FAILED;
|
|
}
|
|
|
|
void AP_Frsky_MAVliteMsgHandler::send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid)
|
|
{
|
|
AP_Frsky_MAVlite_Message txmsg;
|
|
txmsg.msgid = MAVLINK_MSG_ID_COMMAND_ACK;
|
|
if (!txmsg.set_uint16(cmdid, 0)) {
|
|
return;
|
|
}
|
|
if (!txmsg.set_uint8((uint8_t)mav_result, 2)) {
|
|
return;
|
|
}
|
|
send_message(txmsg);
|
|
}
|
|
|
|
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_calibration_baro(const mavlink_command_long_t &mav_command_long)
|
|
{
|
|
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))) {
|
|
return MAV_RESULT_FAILED;
|
|
}
|
|
|
|
if (hal.util->get_soft_armed()) {
|
|
return MAV_RESULT_DENIED;
|
|
}
|
|
// fast barometer calibration
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Updating barometer calibration");
|
|
AP::baro().update_calibration();
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
|
|
|
|
#if AP_AIRSPEED_ENABLED
|
|
AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
|
|
if (airspeed != nullptr) {
|
|
airspeed->calibrate(false);
|
|
}
|
|
#endif
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
}
|
|
|
|
#if AP_FENCE_ENABLED
|
|
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(const mavlink_command_long_t &mav_command_long)
|
|
{
|
|
AC_Fence *fence = AP::fence();
|
|
if (fence == nullptr) {
|
|
return MAV_RESULT_UNSUPPORTED;
|
|
}
|
|
|
|
switch ((uint16_t)mav_command_long.param1) {
|
|
case 0:
|
|
fence->enable(false);
|
|
return MAV_RESULT_ACCEPTED;
|
|
case 1:
|
|
fence->enable(true);
|
|
return MAV_RESULT_ACCEPTED;
|
|
default:
|
|
return MAV_RESULT_FAILED;
|
|
}
|
|
}
|
|
#endif // AP_FENCE_ENABLED
|
|
|
|
/*
|
|
* Handle the PARAM_REQUEST_READ mavlite message
|
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
|
*/
|
|
void AP_Frsky_MAVliteMsgHandler::handle_param_request_read(const AP_Frsky_MAVlite_Message &rxmsg)
|
|
{
|
|
float param_value;
|
|
char param_name[AP_MAX_NAME_SIZE+1];
|
|
if (!rxmsg.get_string(param_name, 0)) {
|
|
return;
|
|
}
|
|
// find existing param
|
|
if (!AP_Param::get(param_name,param_value)) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);
|
|
return;
|
|
}
|
|
AP_Frsky_MAVlite_Message txmsg;
|
|
txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
|
|
if (!txmsg.set_float(param_value, 0)) {
|
|
return;
|
|
}
|
|
if (!txmsg.set_string(param_name, 4)) {
|
|
return;
|
|
}
|
|
send_message(txmsg);
|
|
}
|
|
|
|
/*
|
|
* Handle the PARAM_SET mavlite message
|
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
|
*/
|
|
void AP_Frsky_MAVliteMsgHandler::handle_param_set(const AP_Frsky_MAVlite_Message &rxmsg)
|
|
{
|
|
float param_value;
|
|
char param_name[AP_MAX_NAME_SIZE+1];
|
|
// populate packet with mavlite payload
|
|
if (!rxmsg.get_float(param_value, 0)) {
|
|
return;
|
|
}
|
|
if (!rxmsg.get_string(param_name, 4)) {
|
|
return;
|
|
}
|
|
// find existing param so we can get the old value
|
|
enum ap_var_type var_type;
|
|
// set parameter
|
|
AP_Param *vp;
|
|
uint16_t parameter_flags = 0;
|
|
vp = AP_Param::find(param_name, &var_type, ¶meter_flags);
|
|
if (vp == nullptr || isnan(param_value) || isinf(param_value)) {
|
|
return;
|
|
}
|
|
if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) {
|
|
// the user can set BRD_OPTIONS to enable set of internal
|
|
// parameters, for developer testing or unusual use cases
|
|
if (AP_BoardConfig::allow_set_internal_parameters()) {
|
|
parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY;
|
|
}
|
|
}
|
|
if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", param_name);
|
|
} else if (!AP_Param::set_and_save_by_name(param_name, param_value)) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Param write failed (%s)", param_name);
|
|
}
|
|
// let's read back the last value, either the readonly one or the updated one
|
|
if (!AP_Param::get(param_name,param_value)) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);
|
|
return;
|
|
}
|
|
AP_Frsky_MAVlite_Message txmsg;
|
|
txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
|
|
if (!txmsg.set_float(param_value, 0)) {
|
|
return;
|
|
}
|
|
if (!txmsg.set_string(param_name, 4)) {
|
|
return;
|
|
}
|
|
send_message(txmsg);
|
|
}
|
|
|
|
/*
|
|
Handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command
|
|
for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
|
Optionally disable PX4IO overrides. This is done for quadplanes to
|
|
prevent the mixer running while rebooting which can start the VTOL
|
|
motors. That can be dangerous when a preflight reboot is done with
|
|
the pilot close to the aircraft and can also damage the aircraft
|
|
*/
|
|
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_reboot(const mavlink_command_long_t &mav_command_long)
|
|
{
|
|
if (!(is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2))) {
|
|
return MAV_RESULT_FAILED;
|
|
}
|
|
if (hal.util->get_soft_armed()) {
|
|
// refuse reboot when armed
|
|
return MAV_RESULT_FAILED;
|
|
}
|
|
|
|
AP::vehicle()->reboot(false);
|
|
|
|
return MAV_RESULT_FAILED;
|
|
}
|
|
|
|
/*
|
|
* Process an incoming mavlite message
|
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
|
*/
|
|
void AP_Frsky_MAVliteMsgHandler::process_message(const AP_Frsky_MAVlite_Message &rxmsg)
|
|
{
|
|
switch (rxmsg.msgid) {
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
|
|
handle_param_request_read(rxmsg);
|
|
break;
|
|
case MAVLINK_MSG_ID_PARAM_SET:
|
|
handle_param_set(rxmsg);
|
|
break;
|
|
case MAVLINK_MSG_ID_COMMAND_LONG:
|
|
handle_command_long(rxmsg);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Send a mavlite message
|
|
*/
|
|
bool AP_Frsky_MAVliteMsgHandler::send_message(AP_Frsky_MAVlite_Message &txmsg)
|
|
{
|
|
return _send_fn(txmsg);
|
|
}
|
|
#endif
|