CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp
Views: 1798
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
void AP_Frsky_MAVliteMsgHandler::handle_command_long(const AP_Frsky_MAVlite_Message &rxmsg)

16
{

17
    mavlink_command_long_t mav_command_long {};

18


19
    uint8_t cmd_options;

20
    float params[7] {};

21


22
    if (!rxmsg.get_uint16(mav_command_long.command, 0)) {

23
        return;

24
    }

25
    if (!rxmsg.get_uint8(cmd_options, 2)) {

26
        return;

27
    }

28
    uint8_t param_count = AP_Frsky_MAVlite_Message::bit8_unpack(cmd_options, 3, 0);               // first 3 bits

29


30
    for (uint8_t cmd_idx=0; cmd_idx<param_count; cmd_idx++) {

31
        // base offset is 3, relative offset is 4*cmd_idx

32
        if (!rxmsg.get_float(params[cmd_idx], 3+(4*cmd_idx))) {

33
            return;

34
        }

35
    }

36


37
    mav_command_long.param1 = params[0];

38
    mav_command_long.param2 = params[1];

39
    mav_command_long.param3 = params[2];

40
    mav_command_long.param4 = params[3];

41
    mav_command_long.param5 = params[4];

42
    mav_command_long.param6 = params[5];

43
    mav_command_long.param7 = params[6];

44


45
    const MAV_RESULT mav_result = handle_command(mav_command_long);

46
    send_command_ack(mav_result, mav_command_long.command);

47
}

48


49
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command(const mavlink_command_long_t &mav_command_long)

50
{

51
    // filter allowed commands

52
    switch (mav_command_long.command) {

53
        //case MAV_CMD_ACCELCAL_VEHICLE_POS:

54
        case MAV_CMD_DO_SET_MODE:

55
            return handle_command_do_set_mode(mav_command_long);

56
        //case MAV_CMD_DO_SET_HOME:

57
#if AP_FENCE_ENABLED

58
        case MAV_CMD_DO_FENCE_ENABLE:

59
            return handle_command_do_fence_enable(mav_command_long);

60
#endif

61
        case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:

62
            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
        case MAV_CMD_PREFLIGHT_CALIBRATION:

78
            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
        default:

96
            return MAV_RESULT_UNSUPPORTED;

97
    }

98
    return MAV_RESULT_UNSUPPORTED;

99
}

100


101
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_set_mode(const mavlink_command_long_t &mav_command_long)

102
{

103
    if (AP::vehicle()->set_mode(mav_command_long.param1, ModeReason::FRSKY_COMMAND)) {

104
        return MAV_RESULT_ACCEPTED;

105
    }

106
    return MAV_RESULT_FAILED;

107
}

108


109
void AP_Frsky_MAVliteMsgHandler::send_command_ack(const MAV_RESULT mav_result, const uint16_t cmdid)

110
{

111
    AP_Frsky_MAVlite_Message txmsg;

112
    txmsg.msgid = MAVLINK_MSG_ID_COMMAND_ACK;

113
    if (!txmsg.set_uint16(cmdid, 0)) {

114
        return;

115
    }

116
    if (!txmsg.set_uint8((uint8_t)mav_result, 2)) {

117
        return;

118
    }

119
    send_message(txmsg);

120
}

121


122
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_calibration_baro(const mavlink_command_long_t &mav_command_long)

123
{

124
    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
        return MAV_RESULT_FAILED;

126
    }

127


128
    if (hal.util->get_soft_armed()) {

129
        return MAV_RESULT_DENIED;

130
    }

131
    // fast barometer calibration

132
    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Updating barometer calibration");

133
    AP::baro().update_calibration();

134
    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer calibration complete");

135


136
#if AP_AIRSPEED_ENABLED

137
    AP_Airspeed *airspeed = AP_Airspeed::get_singleton();

138
    if (airspeed != nullptr) {

139
        airspeed->calibrate(false);

140
    }

141
#endif

142


143
    return MAV_RESULT_ACCEPTED;

144
}

145


146
#if AP_FENCE_ENABLED

147
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(const mavlink_command_long_t &mav_command_long)

148
{

149
    AC_Fence *fence = AP::fence();

150
    if (fence == nullptr) {

151
        return MAV_RESULT_UNSUPPORTED;

152
    }

153


154
    switch ((uint16_t)mav_command_long.param1) {

155
        case 0:

156
            fence->enable_configured(false);

157
            return MAV_RESULT_ACCEPTED;

158
        case 1:

159
            fence->enable_configured(true);

160
            return MAV_RESULT_ACCEPTED;

161
        default:

162
            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
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
    if (!rxmsg.get_string(param_name, 0)) {

176
        return;

177
    }

178
    // find existing param

179
    if (!AP_Param::get(param_name,param_value)) {

180
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);

181
        return;

182
    }

183
    AP_Frsky_MAVlite_Message txmsg;

184
    txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;

185
    if (!txmsg.set_float(param_value, 0)) {

186
        return;

187
    }

188
    if (!txmsg.set_string(param_name, 4)) {

189
        return;

190
    }

191
    send_message(txmsg);

192
}

193


194
/*

195
 * Handle the PARAM_SET mavlite message

196
 * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)

197
 */

198
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
    if (!rxmsg.get_float(param_value, 0)) {

204
        return;

205
    }

206
    if (!rxmsg.get_string(param_name, 4)) {

207
        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
    uint16_t parameter_flags = 0;

214
    vp = AP_Param::find(param_name, &var_type, &parameter_flags);

215
    if (vp == nullptr || isnan(param_value) || isinf(param_value)) {

216
        return;

217
    }

218
    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
        if (AP_BoardConfig::allow_set_internal_parameters()) {

222
            parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY;

223
        }

224
    }

225
    if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {

226
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param write denied (%s)", param_name);

227
    } else if (!AP_Param::set_and_save_by_name(param_name, param_value)) {

228
        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
    if (!AP_Param::get(param_name,param_value)) {

232
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param read failed (%s)", param_name);

233
        return;

234
    }

235
    AP_Frsky_MAVlite_Message txmsg;

236
    txmsg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;

237
    if (!txmsg.set_float(param_value, 0)) {

238
        return;

239
    }

240
    if (!txmsg.set_string(param_name, 4)) {

241
        return;

242
    }

243
    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
MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_preflight_reboot(const mavlink_command_long_t &mav_command_long)

255
{

256
    if (!(is_equal(mav_command_long.param1,1.0f) && is_zero(mav_command_long.param2))) {

257
        return MAV_RESULT_FAILED;

258
    }

259
    if (hal.util->get_soft_armed()) {

260
        // refuse reboot when armed

261
        return MAV_RESULT_FAILED;

262
    }

263


264
    AP::vehicle()->reboot(false);

265


266
    return MAV_RESULT_FAILED;

267
}

268


269
/*

270
 * Process an incoming mavlite message

271
 * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)

272
 */

273
void AP_Frsky_MAVliteMsgHandler::process_message(const AP_Frsky_MAVlite_Message &rxmsg)

274
{

275
    switch (rxmsg.msgid) {

276
        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:

277
            handle_param_request_read(rxmsg);

278
            break;

279
        case MAVLINK_MSG_ID_PARAM_SET:

280
            handle_param_set(rxmsg);

281
            break;

282
        case MAVLINK_MSG_ID_COMMAND_LONG:

283
            handle_command_long(rxmsg);

284
            break;

285
    }

286
}

287


288
/*

289
 * Send a mavlite message

290
 */

291
bool AP_Frsky_MAVliteMsgHandler::send_message(AP_Frsky_MAVlite_Message &txmsg)

292
{

293
    return _send_fn(txmsg);

294
}

295
#endif

296


297