/*
send RC_CHANNELS_RAW, and RC_CHANNELS messages
*/
void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi)
{
uint32_t now = hal.scheduler->millis();
uint16_t values[8];
memset(values, 0, sizeof(values));
hal.rcin->read(values, 8);
mavlink_msg_rc_channels_raw_send(
chan,
now,
0, // port
values[0],
values[1],
values[2],
values[3],
values[4],
values[5],
values[6],
values[7],
receiver_rssi);
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
if (hal.rcin->num_channels() > 8 &&
comm_get_txspace(chan) >= MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
mavlink_msg_rc_channels_send(
chan,
now,
hal.rcin->num_channels(),
hal.rcin->read(CH_1),
hal.rcin->read(CH_2),
hal.rcin->read(CH_3),
hal.rcin->read(CH_4),
hal.rcin->read(CH_5),
hal.rcin->read(CH_6),
hal.rcin->read(CH_7),
hal.rcin->read(CH_8),
hal.rcin->read(CH_9),
hal.rcin->read(CH_10),
hal.rcin->read(CH_11),
hal.rcin->read(CH_12),
hal.rcin->read(CH_13),
hal.rcin->read(CH_14),
hal.rcin->read(CH_15),
hal.rcin->read(CH_16),
hal.rcin->read(CH_17),
hal.rcin->read(CH_18),
receiver_rssi);
}
#endif
}
void GCS_MAVLINK::send_home(const Location &home) const
{
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
mavlink_msg_home_position_send(
chan,
home.lat,
home.lng,
home.alt / 100,
0.0f, 0.0f, 0.0f,
q,
0.0f, 0.0f, 0.0f);
}
}
bool try_send_statustext(mavlink_channel_t chan, const char *text, int len) {
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
CHECK_PAYLOAD_SIZE(STATUSTEXT);
char statustext[50] = { 0 };
if (len < 50) {
memcpy(statustext, text, len);
}
mavlink_msg_statustext_send(
chan,
1, /* SEVERITY_LOW */
statustext);
return true;
}
/*
send a statustext message to all active MAVLink connections
*/
void GCS_MAVLINK::send_statustext_all(const prog_char_t *msg)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
char msg2[50];
strncpy_P(msg2, msg, sizeof(msg2));
mavlink_msg_statustext_send(chan,
SEVERITY_HIGH,
msg2);
}
}
}
}
void
GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
{
if (severity != SEVERITY_LOW &&
comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// send immediately
mavlink_msg_statustext_send(chan, severity, str);
} else {
// send via the deferred queuing system
mavlink_statustext_t *s = &pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
send_message(MSG_STATUSTEXT);
}
}
void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...)
{
char msg[50];
va_list ap;
va_start(ap, fmt);
hal.util->vsnprintf_P(msg, sizeof(msg), (const prog_char_t *)fmt, ap);
va_end(ap);
if (msg[strlen(msg)-1] == '\n') {
// STATUSTEXT messages should not add linefeed
msg[strlen(msg)-1] = 0;
}
while (comm_get_txspace(_chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES < sizeof(mavlink_statustext_t)) {
hal.scheduler->delay(1);
}
mavlink_msg_statustext_send(_chan, SEVERITY_USER_RESPONSE, msg);
}
/*
send a parameter value message to all active MAVLink connections
*/
void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
mavlink_msg_param_value_send(
chan,
param_name,
param_value,
mav_var_type(param_type),
AP_Param::count_parameters(),
-1);
}
}
}
}
/*
special handling for heartbeat messages. To ensure routing
propagation heartbeat messages need to be forwarded on all channels
except channels where the sysid/compid of the heartbeat could come from
*/
void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t* msg)
{
if (msg->compid == MAV_COMP_ID_GIMBAL) {
return;
}
uint16_t mask = GCS_MAVLINK::active_channel_mask();
// don't send on the incoming channel. This should only matter if
// the routing table is full
mask &= ~(1U<<(in_channel-MAVLINK_COMM_0));
// mask out channels that do not want the heartbeat to be forwarded
mask &= ~no_route_mask;
// mask out channels that are known sources for this sysid/compid
for (uint8_t i=0; i<num_routes; i++) {
if (routes[i].sysid == msg->sysid && routes[i].compid == msg->compid) {
mask &= ~(1U<<((unsigned)(routes[i].channel-MAVLINK_COMM_0)));
}
}
if (mask == 0) {
// nothing to send to
return;
}
// send on the remaining channels
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if (mask & (1U<<i)) {
mavlink_channel_t channel = (mavlink_channel_t)(MAVLINK_COMM_0 + i);
if (comm_get_txspace(channel) >= ((uint16_t)msg->len) +
GCS_MAVLINK::packet_overhead_chan(channel)) {
#if ROUTING_DEBUG
::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n",
(unsigned)in_channel,
(unsigned)channel,
(unsigned)msg->sysid,
(unsigned)msg->compid);
#endif
_mavlink_resend_uart(channel, msg);
}
}
}
}
/*
send a statustext message to all active MAVLink connections
*/
void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
char msg2[50] {};
va_list arg_list;
va_start(arg_list, fmt);
hal.util->vsnprintf((char *)msg2, sizeof(msg2), fmt, arg_list);
va_end(arg_list);
mavlink_msg_statustext_send(chan,
severity,
msg2);
}
}
}
}
/*
send a statustext message to specific MAVLink connections in a bitmask
*/
void GCS_MAVLINK::service_statustext(void)
{
// create bitmask of what mavlink ports we should send this text to.
// note, if sending to all ports, we only need to store the bitmask for each and the string only once.
// once we send over a link, clear the port but other busy ports bit may stay allowing for faster links
// to clear the bit and send quickly but slower links to still store the string. Regardless of mixed
// bitrates of ports, a maximum of GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY strings can be buffered. Downside
// is if you have a super slow link mixed with a faster port, if there are GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY
// strings in the slow queue then the next item can not be queued for the faster link
if (_statustext_queue.empty()) {
// nothing to do
return;
}
for (uint8_t idx=0; idx<GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY; ) {
statustext_t *statustext = _statustext_queue[idx];
if (statustext == nullptr) {
break;
}
// try and send to all active mavlink ports listed in the statustext.bitmask
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
uint8_t chan_bit = (1U<<i);
// logical AND (&) to mask them together
if (statustext->bitmask & chan_bit) {
// something is queued on a port and that's the port index we're looped at
mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan_index) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// we have space so send then clear that channel bit on the mask
mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text);
statustext->bitmask &= ~chan_bit;
}
}
}
if (statustext->bitmask == 0) {
_statustext_queue.remove(idx);
} else {
// move to next index
idx++;
}
}
}
void GCS_MAVLINK::send_home_all(const Location &home)
{
const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
mavlink_msg_home_position_send(
chan,
home.lat,
home.lng,
home.alt / 100,
0.0f, 0.0f, 0.0f,
q,
0.0f, 0.0f, 0.0f);
}
}
}
}
// locks onto a particular target sysid and sets it's position data stream to at least 1hz
void Tracker::mavlink_check_target(const mavlink_message_t* msg)
{
// exit immediately if the target has already been set
if (target_set) {
return;
}
// decode
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
// exit immediately if this is not a vehicle we would track
if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) ||
(packet.type == MAV_TYPE_GCS) ||
(packet.type == MAV_TYPE_ONBOARD_CONTROLLER) ||
(packet.type == MAV_TYPE_GIMBAL)) {
return;
}
// set our sysid to the target, this ensures we lock onto a single vehicle
if (g.sysid_target == 0) {
g.sysid_target = msg->sysid;
}
// send data stream request to target on all channels
// Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs[i].initialised) {
if (comm_get_txspace((mavlink_channel_t)i) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_DATA_STREAM_LEN) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
msg->sysid,
msg->compid,
MAV_DATA_STREAM_POSITION,
1, // 1hz
1); // start streaming
}
}
}
// flag target has been set
target_set = 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;
}
/**
trigger sending of log messages if there are some pending
*/
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
{
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space < MAVLINK_MSG_ID_LOG_ENTRY_LEN) {
// no space
return;
}
if (hal.scheduler->millis() - last_heartbeat_time > 3000) {
// give a heartbeat a chance
return;
}
uint32_t size, time_utc;
dataflash.get_log_info(_log_next_list_entry, size, time_utc);
mavlink_msg_log_entry_send(chan, _log_next_list_entry, _log_num_logs, _log_last_list_entry, time_utc, size);
if (_log_next_list_entry == _log_last_list_entry) {
_log_listing = false;
} else {
_log_next_list_entry++;
}
}
/*
send a MAVLink message to all components with this vehicle's system id
This is a no-op if no routes to components have been learned
*/
void MAVLink_routing::send_to_components(const mavlink_message_t* msg)
{
bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS];
memset(sent_to_chan, 0, sizeof(sent_to_chan));
// check learned routes
for (uint8_t i=0; i<num_routes; i++) {
if ((routes[i].sysid == mavlink_system.sysid) && !sent_to_chan[routes[i].channel]) {
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
#if ROUTING_DEBUG
::printf("send msg %u on chan %u sysid=%u compid=%u\n",
msg->msgid,
(unsigned)routes[i].channel,
(unsigned)routes[i].sysid,
(unsigned)routes[i].compid);
#endif
_mavlink_resend_uart(routes[i].channel, msg);
sent_to_chan[routes[i].channel] = true;
}
}
}
}
// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK::try_send_message(enum ap_message id)
{
uint16_t txspace = comm_get_txspace(chan);
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
tracker.send_heartbeat(chan);
return true;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
tracker.send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
tracker.send_location(chan);
break;
case MSG_LOCAL_POSITION:
CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
send_local_position(tracker.ahrs);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
tracker.send_nav_controller_output(chan);
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
tracker.gcs[chan-MAVLINK_COMM_0].send_gps_raw(tracker.gps);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
tracker.gcs[chan-MAVLINK_COMM_0].send_radio_in(0);
break;
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
tracker.send_radio_out(chan);
break;
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
tracker.gcs[chan-MAVLINK_COMM_0].send_raw_imu(tracker.ins, tracker.compass);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
tracker.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(tracker.barometer);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
tracker.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(tracker.ins, tracker.compass, tracker.barometer);
break;
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
tracker.gcs[chan-MAVLINK_COMM_0].queued_param_send();
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
tracker.send_waypoint_request(chan);
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
tracker.send_statustext(chan);
break;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
tracker.gcs[chan-MAVLINK_COMM_0].send_ahrs(tracker.ahrs);
break;
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
tracker.send_simstate(chan);
break;
case MSG_HWSTATUS:
CHECK_PAYLOAD_SIZE(HWSTATUS);
tracker.send_hwstatus(chan);
break;
case MSG_SERVO_OUT:
case MSG_EXTENDED_STATUS1:
case MSG_EXTENDED_STATUS2:
case MSG_RETRY_DEFERRED:
case MSG_CURRENT_WAYPOINT:
case MSG_VFR_HUD:
case MSG_SYSTEM_TIME:
case MSG_LIMITS_STATUS:
case MSG_FENCE_STATUS:
case MSG_WIND:
case MSG_RANGEFINDER:
case MSG_TERRAIN:
case MSG_BATTERY2:
case MSG_CAMERA_FEEDBACK:
case MSG_MOUNT_STATUS:
case MSG_OPTICAL_FLOW:
case MSG_GIMBAL_REPORT:
case MSG_EKF_STATUS_REPORT:
case MSG_PID_TUNING:
case MSG_VIBRATION:
case MSG_RPM:
break; // just here to prevent a warning
}
return true;
}
/*
handle an incoming mission item
*/
void GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission)
{
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
struct AP_Mission::Mission_Command cmd = {};
mavlink_msg_mission_item_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// convert mavlink packet to mission command
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
result = MAV_MISSION_INVALID;
goto mission_ack;
}
if (packet.current == 2) {
// current = 2 is a flag to tell us this is a "guided mode"
// waypoint and not for the mission
handle_guided_request(cmd);
// verify we received the command
result = 0;
goto mission_ack;
}
if (packet.current == 3) {
//current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
handle_change_alt_request(cmd);
// verify we recevied the command
result = 0;
goto mission_ack;
}
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_ack;
}
// if command index is within the existing list, replace the command
if (packet.seq < mission.num_commands()) {
if (mission.replace_cmd(packet.seq,cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if command is at the end of command list, add the command
} else if (packet.seq == mission.num_commands()) {
if (mission.add_cmd(cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if beyond the end of the command list, return an error
} else {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// update waypoint receiving state machine
waypoint_timelast_receive = hal.scheduler->millis();
waypoint_request_i++;
if (waypoint_request_i >= waypoint_request_last) {
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
MAV_MISSION_ACCEPTED);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
} else {
waypoint_timelast_request = hal.scheduler->millis();
// if we have enough space, then send the next WP immediately
if (comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
queued_waypoint_send();
} else {
send_message(MSG_NEXT_WAYPOINT);
}
}
return;
mission_ack:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
result);
}
void
GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)
{
if (severity < MAV_SEVERITY_WARNING &&
comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// send immediately
char msg[50] {};
strncpy(msg, str, sizeof(msg));
mavlink_msg_statustext_send(chan, severity, msg);
} else {
// send via the deferred queuing system
mavlink_statustext_t *s = &pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
send_message(MSG_STATUSTEXT);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && APM_BUILD_TYPE(APM_BUILD_ArduCopter)
if (!strcmp(str, "compass disabled")) // Messages defined in compassmot.pde (10x)
textId = 1;
else if (!strcmp(str, "check compass"))
textId = 2;
else if (!strcmp(str, "RC not calibrated"))
textId = 3;
else if (!strcmp(str, "thr not zero"))
textId = 4;
else if (!strcmp(str, "Not landed"))
textId = 5;
else if (!strcmp(str, "STARTING CALIBRATION"))
textId = 6;
else if (!strcmp(str, "CURRENT"))
textId = 7;
else if (!strcmp(str, "THROTTLE"))
textId = 8;
else if (!strcmp(str, "Calibration Successful"))
textId = 9;
else if (!strcmp(str, "Failed"))
textId = 10;
else if (!strcmp(str, "AutoTune: Started")) // Messages defined in control_autotune.pde (4x)
textId = 21;
else if (!strcmp(str, "AutoTune: Stopped"))
textId = 22;
else if (!strcmp(str, "AutoTune: Success"))
textId = 23;
else if (!strcmp(str, "AutoTune: Failed"))
textId = 24;
else if (!strcmp(str, "Crash: Disarming")) // Messages defined in crash_check.pde (3x)
textId = 35;
else if (!strcmp(str, "Parachute: Released"))
textId = 36;
else if (!strcmp(str, "Parachute: Too Low"))
textId = 37;
else if (!strcmp(str, "EKF variance")) // Messages defined in ekf_check.pde (2x)
textId = 48;
else if (!strcmp(str, "DCM bad heading"))
textId = 49;
else if (!strcmp(str, "Low Battery")) // Messages defined in events.pde (2x)
textId = 60;
else if (!strcmp(str, "Lost GPS"))
textId = 61;
else if (!strcmp(str, "bad rally point message ID")) // Messages defined in GCS_Mavlink.pde (6x)
textId = 72;
else if (!strcmp(str, "bad rally point message count"))
textId = 73;
else if (!strcmp(str, "error setting rally point"))
textId = 74;
else if (!strcmp(str, "bad rally point index"))
textId = 75;
else if (!strcmp(str, "failed to set rally point"))
textId = 76;
else if (!strcmp(str, "Initialising APM..."))
textId = 77;
else if (!strcmp(str, "Erasing logs")) // Messages defined in Log.pde (2x)
textId = 88;
else if (!strcmp(str, "Log erase complete"))
textId = 89;
else if (!strcmp(str, "ARMING MOTORS")) // Messages defined in motors.pde (30x)
textId = 100;
else if (!strcmp(str, "Arm: Gyro cal failed"))
textId = 101;
else if (!strcmp(str, "PreArm: RC not calibrated"))
textId = 102;
else if (!strcmp(str, "PreArm: Baro not healthy"))
textId = 103;
else if (!strcmp(str, "PreArm: Alt disparity"))
textId = 104;
else if (!strcmp(str, "PreArm: Compass not healthy"))
textId = 105;
else if (!strcmp(str, "PreArm: Compass not calibrated"))
textId = 106;
else if (!strcmp(str, "PreArm: Compass offsets too high"))
textId = 107;
else if (!strcmp(str, "PreArm: Check mag field"))
textId = 108;
else if (!strcmp(str, "PreArm: compasses inconsistent"))
textId = 109;
else if (!strcmp(str, "PreArm: INS not calibrated"))
textId = 110;
else if (!strcmp(str, "PreArm: Accels not healthy"))
textId = 111;
else if (!strcmp(str, "PreArm: Accels inconsistent"))
textId = 112;
else if (!strcmp(str, "PreArm: Gyros not healthy"))
textId = 113;
else if (!strcmp(str, "PreArm: Gyro cal failed"))
textId = 114;
else if (!strcmp(str, "PreArm: Gyros inconsistent"))
textId = 115;
else if (!strcmp(str, "PreArm: Check Board Voltage"))
textId = 116;
else if (!strcmp(str, "PreArm: Ch7&Ch8 Opt cannot be same"))
textId = 117;
else if (!strcmp(str, "PreArm: Check FS_THR_VALUE"))
textId = 118;
else if (!strcmp(str, "PreArm: Check ANGLE_MAX"))
textId = 119;
else if (!strcmp(str, "PreArm: ACRO_BAL_ROLL/PITCH"))
textId = 120;
else if (!strcmp(str, "PreArm: GPS Glitch"))
textId = 121;
else if (!strcmp(str, "PreArm: Need 3D Fix"))
textId = 122;
else if (!strcmp(str, "PreArm: Bad Velocity"))
textId = 123;
else if (!strcmp(str, "PreArm: High GPS HDOP"))
textId = 124;
else if (!strcmp(str, "Arm: Alt disparity"))
textId = 125;
else if (!strcmp(str, "Arm: Thr below FS"))
textId = 126;
else if (!strcmp(str, "Arm: Leaning"))
textId = 127;
else if (!strcmp(str, "Arm: Safety Switch"))
textId = 128;
else if (!strcmp(str, "DISARMING MOTORS"))
textId = 129;
else if (!strcmp(str, "Motor Test: RC not calibrated")) // Messages defined in motor_test.pde (3x)
textId = 140;
else if (!strcmp(str, "Motor Test: vehicle not landed"))
textId = 141;
else if (!strcmp(str, "Motor Test: Safety Switch"))
textId = 142;
else if (!strcmp(str, "Calibrating barometer")) // Messages defined in sensors.pde (2x)
textId = 153;
else if (!strcmp(str, "barometer calibration complete"))
textId = 154;
else if (!strcmp(str, "Trim saved")) // Messages defined in switches.pde (1x)
textId = 165;
else if (!strcmp(str, "No dataflash inserted")) // Messages defined in system.pde (4x)
textId = 176;
else if (!strcmp(str, "ERASING LOGS"))
textId = 177;
else if (!strcmp(str, "Waiting for first HIL_STATE message"))
textId = 178;
else if (!strcmp(str, "GROUND START"))
textId = 179;
else if (!strcmp(str, "PreArm: Baro not healthy")) // Messages defined in AP_Arming.cpp (9x)
textId = 190;
else if (!strcmp(str, "PreArm: Compass not healthy"))
textId = 191;
else if (!strcmp(str, "PreArm: Compass not calibrated"))
textId = 192;
else if (!strcmp(str, "PreArm: Bad GPS Pos"))
textId = 193;
else if (!strcmp(str, "PreArm: Battery failsafe on"))
textId = 194;
else if (!strcmp(str, "PreArm: Hardware Safety Switch"))
textId = 195;
else if (!strcmp(str, "PreArm: Radio failsafe on"))
textId = 196;
else if (!strcmp(str, "Throttle armed"))
textId = 197;
else if (!strcmp(str, "Throttle disarmed"))
textId = 198;
else if (!strcmp(str, "flight plan update rejected")) // Messages defined in GCS_Common.cpp (2x)
textId = 209;
else if (!strcmp(str, "flight plan received"))
textId = 210;
else
textId = 0;
g_severity = severity;
#endif
}
/*
forward a MAVLink message to the right port. This also
automatically learns the route for the sender if it is not
already known.
This returns true if the message should be processed locally
Theory of MAVLink routing:
When a flight controller receives a message it should process it
locally if any of these conditions hold:
1a) the message has no target_system field
1b) the message has a target_system of zero
1c) the message has the flight controllers target system and has no
target_component field
1d) the message has the flight controllers target system and has
the flight controllers target_component
1e) the message has the flight controllers target system and the
flight controller has not seen any messages on any of its links
from a system that has the messages
target_system/target_component combination
When a flight controller receives a message it should forward it
onto another different link if any of these conditions hold for that
link:
2a) the message has no target_system field
2b) the message has a target_system of zero
2c) the message does not have the flight controllers target_system
and the flight controller has seen a message from the messages
target_system on the link
2d) the message has the flight controllers target_system and has a
target_component field and the flight controllers has seen a
message from the target_system/target_component combination on
the link
Note: This proposal assumes that ground stations will not send command
packets to a non-broadcast destination (sysid/compid combination)
until they have received at least one package from that destination
over the link. This is essential to prevent a flight controller from
acting on a message that is not meant for it. For example, a PARAM_SET
cannot be sent to a specific sysid/compid combination until the GCS
has seen a packet from that sysid/compid combination on the link.
The GCS must also reset what sysid/compid combinations it has seen on
a link when it sees a SYSTEM_TIME message with a decrease in
time_boot_ms from a particular sysid/compid. That is essential to
detect a reset of the flight controller, which implies a reset of its
routing table.
*/
bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t* msg)
{
// handle the case of loopback of our own messages, due to
// incorrect serial configuration.
if (msg->sysid == mavlink_system.sysid &&
msg->compid == mavlink_system.compid) {
return true;
}
// learn new routes
learn_route(in_channel, msg);
if (msg->msgid == MAVLINK_MSG_ID_RADIO ||
msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
// don't forward RADIO packets
return true;
}
if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) {
// heartbeat needs special handling
handle_heartbeat(in_channel, msg);
return true;
}
// extract the targets for this packet
int16_t target_system = -1;
int16_t target_component = -1;
get_targets(msg, target_system, target_component);
bool broadcast_system = (target_system == 0 || target_system == -1);
bool broadcast_component = (target_component == 0 || target_component == -1);
bool match_system = broadcast_system || (target_system == mavlink_system.sysid);
bool match_component = match_system && (broadcast_component ||
(target_component == mavlink_system.compid));
bool process_locally = match_system && match_component;
if (process_locally && !broadcast_system && !broadcast_component) {
// nothing more to do - it can only be for us
return true;
}
// forward on any channels matching the targets
bool forwarded = false;
bool sent_to_chan[MAVLINK_COMM_NUM_BUFFERS];
memset(sent_to_chan, 0, sizeof(sent_to_chan));
for (uint8_t i=0; i<num_routes; i++) {
if (broadcast_system || (target_system == routes[i].sysid &&
(broadcast_component ||
target_component == routes[i].compid ||
!match_system))) {
if (in_channel != routes[i].channel && !sent_to_chan[routes[i].channel]) {
if (comm_get_txspace(routes[i].channel) >= ((uint16_t)msg->len) +
GCS_MAVLINK::packet_overhead_chan(routes[i].channel)) {
#if ROUTING_DEBUG
::printf("fwd msg %u from chan %u on chan %u sysid=%d compid=%d\n",
msg->msgid,
(unsigned)in_channel,
(unsigned)routes[i].channel,
(int)target_system,
(int)target_component);
#endif
_mavlink_resend_uart(routes[i].channel, msg);
}
sent_to_chan[routes[i].channel] = true;
forwarded = true;
}
}
}
if (!forwarded && match_system) {
process_locally = true;
}
return process_locally;
}
// try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
{
if (telemetry_delayed()) {
return false;
}
// if we don't have at least 0.2ms remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
if (!hal.scheduler->in_delay_callback() &&
rover.scheduler.time_available_usec() < 200) {
gcs().set_out_of_time(true);
return false;
}
switch (id) {
case MSG_EXTENDED_STATUS1:
// send extended status only once vehicle has been initialised
// to avoid unnecessary errors being reported to user
if (initialised) {
if (PAYLOAD_SIZE(chan, SYS_STATUS) +
PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) {
return false;
}
rover.send_sys_status(chan);
send_power_status();
}
break;
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
rover.send_nav_controller_output(chan);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
rover.send_servo_out(chan);
break;
case MSG_RANGEFINDER:
CHECK_PAYLOAD_SIZE(RANGEFINDER);
rover.send_rangefinder(chan);
send_distance_sensor();
send_proximity();
break;
case MSG_RPM:
CHECK_PAYLOAD_SIZE(RPM);
rover.send_wheel_encoder(chan);
break;
case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
rover.send_fence_status(chan);
break;
case MSG_WIND:
CHECK_PAYLOAD_SIZE(WIND);
rover.g2.windvane.send_wind(chan);
break;
case MSG_PID_TUNING:
CHECK_PAYLOAD_SIZE(PID_TUNING);
rover.send_pid_tuning(chan);
break;
default:
return GCS_MAVLINK::try_send_message(id);
}
return true;
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void Plane::set_servos(void)
{
int16_t last_throttle = channel_throttle->radio_out;
if (control_mode == AUTO && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
return;
}
/*
see if we are doing ground steering.
*/
if (!steering_control.ground_steering) {
// we are not at an altitude for ground steering. Set the nose
// wheel to the rudder just in case the barometer has drifted
// a lot
steering_control.steering = steering_control.rudder;
} else if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_steering)) {
// we are within the ground steering altitude but don't have a
// dedicated steering channel. Set the rudder to the ground
// steering output
steering_control.rudder = steering_control.steering;
}
channel_rudder->servo_out = steering_control.rudder;
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_rudder, steering_control.rudder);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
// do a direct pass through of radio values
if (g.mix_mode == 0 || g.elevon_output != MIXING_DISABLED) {
channel_roll->radio_out = channel_roll->radio_in;
channel_pitch->radio_out = channel_pitch->radio_in;
} else {
channel_roll->radio_out = channel_roll->read();
channel_pitch->radio_out = channel_pitch->read();
}
channel_throttle->radio_out = channel_throttle->radio_in;
channel_rudder->radio_out = channel_rudder->radio_in;
// setup extra channels. We want this to come from the
// main input channel, but using the 2nd channels dead
// zone, reverse and min/max settings. We need to use
// pwm_to_angle_dz() to ensure we don't trim the value for the
// deadzone of the main aileron channel, otherwise the 2nd
// aileron won't quite follow the first one
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->pwm_to_angle_dz(0));
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->pwm_to_angle_dz(0));
// this variant assumes you have the corresponding
// input channel setup in your transmitter for manual control
// of the 2nd aileron
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_elevator_with_input);
if (g.mix_mode == 0 && g.elevon_output == MIXING_DISABLED) {
// set any differential spoilers to follow the elevons in
// manual mode.
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler1, channel_roll->radio_out);
RC_Channel_aux::set_radio(RC_Channel_aux::k_dspoiler2, channel_pitch->radio_out);
}
} else {
if (g.mix_mode == 0) {
// both types of secondary aileron are slaved to the roll servo out
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron, channel_roll->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_aileron_with_input, channel_roll->servo_out);
// both types of secondary elevator are slaved to the pitch servo out
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator, channel_pitch->servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_elevator_with_input, channel_pitch->servo_out);
} else {
/*Elevon mode*/
float ch1;
float ch2;
ch1 = channel_pitch->servo_out - (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
ch2 = channel_pitch->servo_out + (BOOL_TO_SIGN(g.reverse_elevons) * channel_roll->servo_out);
/* Differential Spoilers
If differential spoilers are setup, then we translate
rudder control into splitting of the two ailerons on
the side of the aircraft where we want to induce
additional drag.
*/
if (RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler1) && RC_Channel_aux::function_assigned(RC_Channel_aux::k_dspoiler2)) {
float ch3 = ch1;
float ch4 = ch2;
if ( BOOL_TO_SIGN(g.reverse_elevons) * channel_rudder->servo_out < 0) {
ch1 += abs(channel_rudder->servo_out);
ch3 -= abs(channel_rudder->servo_out);
} else {
ch2 += abs(channel_rudder->servo_out);
ch4 -= abs(channel_rudder->servo_out);
}
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler1, ch3);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_dspoiler2, ch4);
}
// directly set the radio_out values for elevon mode
channel_roll->radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0f/ SERVO_MAX));
channel_pitch->radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0f/ SERVO_MAX));
}
// push out the PWM values
if (g.mix_mode == 0) {
channel_roll->calc_pwm();
channel_pitch->calc_pwm();
}
channel_rudder->calc_pwm();
#if THROTTLE_OUT == 0
channel_throttle->servo_out = 0;
#else
// convert 0 to 100% into PWM
uint8_t min_throttle = aparm.throttle_min.get();
uint8_t max_throttle = aparm.throttle_max.get();
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
min_throttle = 0;
}
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
if(aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max;
} else {
max_throttle = aparm.throttle_max;
}
}
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
min_throttle,
max_throttle);
if (!hal.util->get_soft_armed()) {
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();
} else if (suppress_throttle()) {
// throttle is suppressed in auto mode
channel_throttle->servo_out = 0;
if (g.throttle_suppress_manual) {
// manual pass through of throttle while throttle is suppressed
channel_throttle->radio_out = channel_throttle->radio_in;
} else {
channel_throttle->calc_pwm();
}
} else if (g.throttle_passthru_stabilize &&
(control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE)) {
// manual pass through of throttle while in FBWA or
// STABILIZE mode with THR_PASS_STAB set
channel_throttle->radio_out = channel_throttle->radio_in;
} else if (control_mode == GUIDED &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->radio_out = channel_throttle->radio_in;
} else {
// normal throttle calculation based on servo_out
channel_throttle->calc_pwm();
}
#endif
}
// Auto flap deployment
int8_t auto_flap_percent = 0;
int8_t manual_flap_percent = 0;
static int8_t last_auto_flap;
static int8_t last_manual_flap;
// work out any manual flap input
RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1);
if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) {
flapin->input();
manual_flap_percent = flapin->percent_input();
}
if (auto_throttle_mode) {
int16_t flapSpeedSource = 0;
if (ahrs.airspeed_sensor_enabled()) {
flapSpeedSource = target_airspeed_cm * 0.01f;
} else {
flapSpeedSource = aparm.throttle_cruise;
}
if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
auto_flap_percent = g.flap_2_percent;
} else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) {
auto_flap_percent = g.flap_1_percent;
} //else flaps stay at default zero deflection
/*
special flap levels for takeoff and landing. This works
better than speed based flaps as it leads to less
possibility of oscillation
*/
if (control_mode == AUTO) {
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
if (g.takeoff_flap_percent != 0) {
auto_flap_percent = g.takeoff_flap_percent;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
if (g.land_flap_percent != 0) {
auto_flap_percent = g.land_flap_percent;
}
break;
default:
break;
}
}
}
// manual flap input overrides auto flap input
if (abs(manual_flap_percent) > auto_flap_percent) {
auto_flap_percent = manual_flap_percent;
}
flap_slew_limit(last_auto_flap, auto_flap_percent);
flap_slew_limit(last_manual_flap, manual_flap_percent);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap, manual_flap_percent);
if (control_mode >= FLY_BY_WIRE_B) {
/* only do throttle slew limiting in modes where throttle
* control is automatic */
throttle_slew_limit(last_throttle);
}
if (control_mode == TRAINING) {
// copy rudder in training mode
channel_rudder->radio_out = channel_rudder->radio_in;
}
if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) {
flaperon_update(auto_flap_percent);
}
if (g.vtail_output != MIXING_DISABLED) {
channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out);
} else if (g.elevon_output != MIXING_DISABLED) {
channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out);
}
//send throttle to 0 or MIN_PWM if not yet armed
if (!arming.is_armed()) {
//Some ESCs get noisy (beep error msgs) if PWM == 0.
//This little segment aims to avoid this.
switch (arming.arming_required()) {
case AP_Arming::YES_MIN_PWM:
channel_throttle->radio_out = channel_throttle->radio_min;
break;
case AP_Arming::YES_ZERO_PWM:
channel_throttle->radio_out = 0;
break;
default:
//keep existing behavior: do nothing to radio_out
//(don't disarm throttle channel even if AP_Arming class is)
break;
}
}
#if OBC_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
obc.check_crash_plane();
#endif
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// get the servos to the GCS immediately for HIL
if (comm_get_txspace(MAVLINK_COMM_0) >=
MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
send_servo_out(MAVLINK_COMM_0);
}
if (!g.hil_servos) {
return;
}
}
#endif
// send values to the PWM timers for output
// ----------------------------------------
if (g.rudder_only == 0) {
// when we RUDDER_ONLY mode we don't send the channel_roll
// output and instead rely on KFF_RDDRMIX. That allows the yaw
// damper to operate.
channel_roll->output();
}
channel_pitch->output();
channel_throttle->output();
channel_rudder->output();
RC_Channel_aux::output_ch_all();
}
