/*
This varient of _send() can be used to save stack space by re-using
memory from the receive buffer. The caller provides a
mavlink_message_t which is the size of a full mavlink message. This
is usually the receive buffer for the channel, and allows a reply to an
incoming message with minimum stack space usage.
*/
void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char *buf = (char *)msgbuf;
_mav_put_uint16_t(buf, 0, seq);
_mav_put_uint8_t(buf, 2, target_system);
_mav_put_uint8_t(buf, 3, target_component);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
#endif
#else
mavlink_mission_request_t *packet = (mavlink_mission_request_t *)msgbuf;
packet->seq = seq;
packet->target_system = target_system;
packet->target_component = target_component;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
#endif
#endif
}
/*
This varient of _send() can be used to save stack space by re-using
memory from the receive buffer. The caller provides a
mavlink_message_t which is the size of a full mavlink message. This
is usually the receive buffer for the channel, and allows a reply to an
incoming message with minimum stack space usage.
*/
void _send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char *buf = (char *)msgbuf;
_mav_put_float(buf, 0, airspeed);
_mav_put_float(buf, 4, groundspeed);
_mav_put_float(buf, 8, alt);
_mav_put_float(buf, 12, climb);
_mav_put_int16_t(buf, 16, heading);
_mav_put_uint16_t(buf, 18, throttle);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN);
#endif
#else
mavlink_vfr_hud_t *packet = (mavlink_vfr_hud_t *)msgbuf;
packet->airspeed = airspeed;
packet->groundspeed = groundspeed;
packet->alt = alt;
packet->climb = climb;
packet->heading = heading;
packet->throttle = throttle;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN);
#endif
#endif
}
/*
This varient of _send() can be used to save stack space by re-using
memory from the receive buffer. The caller provides a
mavlink_message_t which is the size of a full mavlink message. This
is usually the receive buffer for the channel, and allows a reply to an
incoming message with minimum stack space usage.
*/
void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char *buf = (char *)msgbuf;
_mav_put_uint32_t(buf, 0, onboard_control_sensors_present);
_mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled);
_mav_put_uint32_t(buf, 8, onboard_control_sensors_health);
_mav_put_uint16_t(buf, 12, load);
_mav_put_uint16_t(buf, 14, voltage_battery);
_mav_put_int16_t(buf, 16, current_battery);
_mav_put_uint16_t(buf, 18, drop_rate_comm);
_mav_put_uint16_t(buf, 20, errors_comm);
_mav_put_uint16_t(buf, 22, errors_count1);
_mav_put_uint16_t(buf, 24, errors_count2);
_mav_put_uint16_t(buf, 26, errors_count3);
_mav_put_uint16_t(buf, 28, errors_count4);
_mav_put_int8_t(buf, 30, battery_remaining);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN);
#endif
#else
mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf;
packet->onboard_control_sensors_present = onboard_control_sensors_present;
packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled;
packet->onboard_control_sensors_health = onboard_control_sensors_health;
packet->load = load;
packet->voltage_battery = voltage_battery;
packet->current_battery = current_battery;
packet->drop_rate_comm = drop_rate_comm;
packet->errors_comm = errors_comm;
packet->errors_count1 = errors_count1;
packet->errors_count2 = errors_count2;
packet->errors_count3 = errors_count3;
packet->errors_count4 = errors_count4;
packet->battery_remaining = battery_remaining;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
#endif
#endif
}
/*
handle a MISSION_REQUEST mavlink packet
*/
void GCS_MAVLINK::handle_mission_request(AP_Mission &mission, mavlink_message_t *msg)
{
AP_Mission::Mission_Command cmd;
// decode
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
return;
}
// retrieve mission from eeprom
if (!mission.read_cmd_from_storage(packet.seq, cmd)) {
goto mission_item_send_failed;
}
// convert mission command to mavlink mission item packet
mavlink_mission_item_t ret_packet;
memset(&ret_packet, 0, sizeof(ret_packet));
if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet)) {
goto mission_item_send_failed;
}
// set packet's current field to 1 if this is the command being executed
if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) {
ret_packet.current = 1;
} else {
ret_packet.current = 0;
}
// set auto continue to 1
ret_packet.autocontinue = 1; // 1 (true), 0 (false)
/*
avoid the _send() function to save memory on APM2, as it avoids
the stack usage of the _send() function by using the already
declared ret_packet above
*/
ret_packet.target_system = msg->sysid;
ret_packet.target_component = msg->compid;
ret_packet.seq = packet.seq;
ret_packet.command = cmd.id;
_mav_finalize_message_chan_send(chan,
MAVLINK_MSG_ID_MISSION_ITEM,
(const char *)&ret_packet,
MAVLINK_MSG_ID_MISSION_ITEM_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_CRC);
return;
mission_item_send_failed:
// send failure message
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR);
}
/**
trigger sending of log data if there are some pending
*/
bool DataFlash_Class::handle_log_send_data()
{
if (!HAVE_PAYLOAD_SPACE(_log_sending_link->get_chan(), LOG_DATA)) {
// no space
return false;
}
if (AP_HAL::millis() - _log_sending_link->get_last_heartbeat_time() > 3000) {
// give a heartbeat a chance
return false;
}
int16_t ret = 0;
uint32_t len = _log_data_remaining;
mavlink_log_data_t packet;
if (len > MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
len = MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN;
}
ret = get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
if (ret < 0) {
// report as EOF on error
ret = 0;
}
if (ret < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN) {
memset(&packet.data[ret], 0, MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN-ret);
}
packet.ofs = _log_data_offset;
packet.id = _log_num_data;
packet.count = ret;
_mav_finalize_message_chan_send(_log_sending_link->get_chan(),
MAVLINK_MSG_ID_LOG_DATA,
(const char *)&packet,
MAVLINK_MSG_ID_LOG_DATA_MIN_LEN,
MAVLINK_MSG_ID_LOG_DATA_LEN,
MAVLINK_MSG_ID_LOG_DATA_CRC);
_log_data_offset += len;
_log_data_remaining -= len;
if (ret < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN || _log_data_remaining == 0) {
transfer_activity = IDLE;
_log_sending_link = nullptr;
}
return true;
}
/**
trigger sending of log data if there are some pending
*/
bool DataFlash_Class::handle_log_send_data(GCS_MAVLINK &link)
{
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), LOG_DATA)) {
// no space
return false;
}
if (AP_HAL::millis() - link.get_last_heartbeat_time() > 3000) {
// give a heartbeat a chance
return false;
}
int16_t ret = 0;
uint32_t len = _log_data_remaining;
mavlink_log_data_t packet;
if (len > 90) {
len = 90;
}
ret = get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
if (ret < 0) {
// report as EOF on error
ret = 0;
}
if (ret < 90) {
memset(&packet.data[ret], 0, 90-ret);
}
packet.ofs = _log_data_offset;
packet.id = _log_num_data;
packet.count = ret;
_mav_finalize_message_chan_send(link.get_chan(), MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet,
MAVLINK_MSG_ID_LOG_DATA_MIN_LEN,
MAVLINK_MSG_ID_LOG_DATA_LEN,
MAVLINK_MSG_ID_LOG_DATA_CRC);
_log_data_offset += len;
_log_data_remaining -= len;
if (ret < 90 || _log_data_remaining == 0) {
_log_sending = false;
_log_sending_chan = -1;
}
return true;
}
/**
trigger sending of log data if there are some pending
*/
bool GCS_MAVLINK::handle_log_send_data(DataFlash_Class &dataflash)
{
uint16_t txspace = comm_get_txspace(chan);
if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_LOG_DATA_LEN) {
// no space
return false;
}
if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
// give a heartbeat a chance
return false;
}
int16_t ret = 0;
uint32_t len = _log_data_remaining;
mavlink_log_data_t packet;
if (len > 90) {
len = 90;
}
ret = dataflash.get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data);
if (ret < 0) {
// report as EOF on error
ret = 0;
}
if (ret < 90) {
memset(&packet.data[ret], 0, 90-ret);
}
packet.ofs = _log_data_offset;
packet.id = _log_num_data;
packet.count = ret;
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet,
MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
_log_data_offset += len;
_log_data_remaining -= len;
if (ret < 90 || _log_data_remaining == 0) {
_log_sending = false;
}
return true;
}
/**
handle a SERIAL_CONTROL message
*/
void GCS_MAVLINK::handle_serial_control(mavlink_message_t *msg, AP_GPS &gps)
{
mavlink_serial_control_t packet;
mavlink_msg_serial_control_decode(msg, &packet);
AP_HAL::UARTDriver *port = NULL;
if (packet.flags & SERIAL_CONTROL_FLAG_REPLY) {
// how did this packet get to us?
return;
}
bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;
switch (packet.device) {
case SERIAL_CONTROL_DEV_TELEM1:
port = hal.uartC;
lock_channel(MAVLINK_COMM_1, exclusive);
break;
case SERIAL_CONTROL_DEV_TELEM2:
port = hal.uartD;
lock_channel(MAVLINK_COMM_2, exclusive);
break;
case SERIAL_CONTROL_DEV_GPS1:
port = hal.uartB;
gps.lock_port(0, exclusive);
break;
case SERIAL_CONTROL_DEV_GPS2:
port = hal.uartE;
gps.lock_port(1, exclusive);
break;
default:
// not supported yet
return;
}
if (exclusive) {
// force flow control off for exclusive access. This protocol
// is used to talk to bootloaders which may not have flow
// control support
port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
}
// optionally change the baudrate
if (packet.baudrate != 0) {
port->begin(packet.baudrate);
}
// write the data
if (packet.count != 0) {
if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) {
port->write(packet.data, packet.count);
} else {
const uint8_t *data = &packet.data[0];
uint8_t count = packet.count;
while (count > 0) {
while (port->txspace() <= 0) {
hal.scheduler->delay(5);
}
uint16_t n = port->txspace();
if (n > packet.count) {
n = packet.count;
}
port->write(data, n);
data += n;
count -= n;
}
}
}
if ((packet.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
// no response expected
return;
}
uint8_t flags = packet.flags;
more_data:
// sleep for the timeout
while (packet.timeout != 0 &&
port->available() < (int16_t)sizeof(packet.data)) {
hal.scheduler->delay(1);
packet.timeout--;
}
packet.flags = SERIAL_CONTROL_FLAG_REPLY;
// work out how many bytes are available
int16_t available = port->available();
if (available < 0) {
available = 0;
}
if (available > (int16_t)sizeof(packet.data)) {
available = sizeof(packet.data);
}
// read any reply data
packet.count = 0;
memset(packet.data, 0, sizeof(packet.data));
while (available > 0) {
packet.data[packet.count++] = (uint8_t)port->read();
available--;
}
// and send the reply
_mav_finalize_message_chan_send(chan,
MAVLINK_MSG_ID_SERIAL_CONTROL,
(const char *)&packet,
MAVLINK_MSG_ID_SERIAL_CONTROL_LEN,
MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
if ((flags & SERIAL_CONTROL_FLAG_MULTI) && packet.count != 0) {
hal.scheduler->delay(1);
goto more_data;
}
}
