/* 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(); }