void send(const hrt_abstime t)
{
(void)status_sub->update(t);
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
}
}
// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode)
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
// reverse pitch and yaw control
pitch_deg = -pitch_deg;
yaw_deg = -yaw_deg;
// send command_long command containing a do_mount_control command
mavlink_msg_command_long_send(_chan,
AP_MOUNT_STORM32_SYSID,
AP_MOUNT_STORM32_COMPID,
MAV_CMD_DO_MOUNT_CONTROL,
0, // confirmation of zero means this is the first time this message has been sent
pitch_deg,
roll_deg,
yaw_deg,
0, 0, 0, // param4 ~ param6 unused
mount_mode);
// store time of send
_last_send = hal.scheduler->millis();
}
void Aircraft::cancelCompassCalibration()
{
m_magCalStatus = MAG_CAL_NOT_STARTED;
m_magCalPercentage = 0;
mavlink_msg_command_long_send(MAVLINK_COMM_0, m_mavSystem, m_mavComponent, MAV_CMD_DO_CANCEL_MAG_CAL, 0, 0, 0, 0, 0, 0, 0, 0);
}
void FMStateMachine::_send_loiter() {
hal.console->println_P(PSTR("FollowMe: Sending loiter cmd packet"));
mavlink_msg_command_long_send(
upstream_channel, /* mavlink_channel_t chan */
_target_system, /* uint8_t target_system */
_target_component, /* uint8_t target_component */
MAV_CMD_NAV_LOITER_UNLIM, /* uint16_t command: arducopter specific */
0, /* uint8_t confirmation */
0, /* float param1 */
0, /* float param2 */
0, /* float param3 */
0, /* float param4 */
0, /* float param5 */
0, /* float param6 */
0 /* float param7 */
);
}
void Aircraft::beginCompassCalibration()
{
m_magCalStatus = MAG_CAL_WAITING_TO_START;
m_magCalPercentage = 0;
mavlink_msg_command_long_send(MAVLINK_COMM_0,
m_mavSystem,
m_mavComponent,
MAV_CMD_DO_START_MAG_CAL,
0, //
1, // uint8_t bitmask of magnetometers (0 means all)
0, // Automatically retry on failure (0=no retry, 1=retry).
0, // Save without user input (0=require input, 1=autosave).
0, // Delay (seconds)
0, // Autoreboot (0=user reboot, 1=autoreboot)
0,
0);
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
if (_cmd_sub->update(t)) {
/* only send commands for other systems/components */
if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
_cmd->target_system,
_cmd->target_component,
_cmd->command,
_cmd->confirmation,
_cmd->param1,
_cmd->param2,
_cmd->param3,
_cmd->param4,
_cmd->param5,
_cmd->param6,
_cmd->param7);
}
}
}
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
struct vehicle_command_s cmd;
if (_cmd_sub->update(&_cmd_time, &cmd)) {
/* only send commands for other systems/components */
if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
cmd.target_system,
cmd.target_component,
cmd.command,
cmd.confirmation,
cmd.param1,
cmd.param2,
cmd.param3,
cmd.param4,
cmd.param5,
cmd.param6,
cmd.param7);
}
}
}
void AP_Gimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg)
{
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
if (flashing() && packet.param_value == 1 && !strcmp(packet.param_id, "GMB_FLASH")) {
mavlink_msg_command_long_send(_chan, 0, MAV_COMP_ID_GIMBAL, 42501, 0, 0, 0, 0, 0, 0, 0, 0);
reset();
return;
}
dataflash->Log_Write_Parameter(packet.param_id, packet.param_value);
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
_params[i].seen = true;
switch(_params[i].state) {
case GMB_PARAMSTATE_NONEXISTANT:
case GMB_PARAMSTATE_NOT_YET_READ:
case GMB_PARAMSTATE_FETCH_AGAIN:
_params[i].value = packet.param_value;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
break;
case GMB_PARAMSTATE_CONSISTENT:
_params[i].value = packet.param_value;
break;
case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
if (i == GMB_PARAM_GMB_FLASH || packet.param_value == _params[i].value) {
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
}
break;
}
break;
}
}
}
void Aircraft::acceptCompassCalibration()
{
mavlink_msg_command_long_send(MAVLINK_COMM_0, m_mavSystem, m_mavComponent, MAV_CMD_DO_ACCEPT_MAG_CAL, 0, 0, 0, 0, 0, 0, 0, 0);
}
