mirror of https://github.com/ArduPilot/ardupilot
611 lines
19 KiB
Plaintext
611 lines
19 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
|
|
|
#include "Mavlink_Common.h"
|
|
|
|
GCS_MAVLINK::GCS_MAVLINK() :
|
|
packet_drops(0)
|
|
{
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::init(BetterStream * port)
|
|
{
|
|
GCS_Class::init(port);
|
|
mavlink_comm_1_port = port;
|
|
chan = MAVLINK_COMM_1;
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::update(void)
|
|
{
|
|
// recieve new packets
|
|
mavlink_message_t msg;
|
|
mavlink_status_t status;
|
|
|
|
// process received bytes
|
|
while(comm_get_available(chan))
|
|
{
|
|
uint8_t c = comm_receive_ch(chan);
|
|
|
|
// Try to get a new message
|
|
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
|
|
}
|
|
|
|
// Update packet drops counter
|
|
packet_drops += status.packet_rx_drop_count;
|
|
|
|
// send out queued params/ waypoints
|
|
mavlink_queued_send(chan);
|
|
|
|
// send parameters communication_queued_send(chan);
|
|
// stop waypoint sending if timeout
|
|
if (global_data.waypoint_sending &&
|
|
millis() - global_data.waypoint_timelast_send >
|
|
global_data.waypoint_send_timeout)
|
|
{
|
|
send_text(SEVERITY_LOW,"waypoint send timeout");
|
|
global_data.waypoint_sending = false;
|
|
}
|
|
|
|
// stop waypoint receiving if timeout
|
|
if (global_data.waypoint_receiving &&
|
|
millis() - global_data.waypoint_timelast_receive >
|
|
global_data.waypoint_receive_timeout)
|
|
{
|
|
send_text(SEVERITY_LOW,"waypoint receive timeout");
|
|
global_data.waypoint_receiving = false;
|
|
}
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
|
|
{
|
|
if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax))
|
|
send_message(MSG_RAW_IMU);
|
|
|
|
if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax))
|
|
send_message(MSG_EXTENDED_STATUS);
|
|
|
|
if (freqLoopMatch(global_data.streamRateRCChannels,freqMin,freqMax))
|
|
send_message(MSG_RADIO_OUT);
|
|
|
|
if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax))
|
|
send_message(MSG_SERVO_OUT);
|
|
|
|
//if (freqLoopMatch(global_data.streamRateRawSensorFusion,freqMin,freqMax))
|
|
|
|
if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax))
|
|
send_message(MSG_LOCATION);
|
|
|
|
if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax))
|
|
send_message(MSG_GPS_STATUS);
|
|
|
|
if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax))
|
|
send_message(MSG_CURRENT_WAYPOINT);
|
|
|
|
if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax))
|
|
{
|
|
send_message(MSG_LOCAL_LOCATION);
|
|
send_message(MSG_ATTITUDE);
|
|
}
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
|
|
{
|
|
mavlink_send_message(chan,id,param,packet_drops);
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::send_text(uint8_t severity, const char *str)
|
|
{
|
|
mavlink_send_text(chan,severity,str);
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
|
|
{
|
|
mavlink_acknowledge(chan,id,sum1,sum2);
|
|
}
|
|
|
|
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
{
|
|
switch (msg->msgid) {
|
|
|
|
case MAVLINK_MSG_ID_GLOBAL_POSITION:
|
|
{
|
|
// decode
|
|
mavlink_global_position_t packet;
|
|
mavlink_msg_global_position_decode(msg, &packet);
|
|
//if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
trackVehicle_loc.id = 0;
|
|
trackVehicle_loc.p1 = 0;
|
|
trackVehicle_loc.alt = packet.alt;
|
|
trackVehicle_loc.lat = packet.lat;
|
|
trackVehicle_loc.lng = packet.lon;
|
|
Serial.println("received global position packet");
|
|
}
|
|
|
|
|
|
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
|
|
{
|
|
// decode
|
|
mavlink_request_data_stream_t packet;
|
|
mavlink_msg_request_data_stream_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
int freq = 0; // packet frequency
|
|
|
|
if (packet.start_stop == 0) freq = 0; // stop sending
|
|
else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending
|
|
else break;
|
|
|
|
switch(packet.req_stream_id)
|
|
{
|
|
case MAV_DATA_STREAM_ALL:
|
|
global_data.streamRateRawSensors = freq;
|
|
global_data.streamRateExtendedStatus = freq;
|
|
global_data.streamRateRCChannels = freq;
|
|
global_data.streamRateRawController = freq;
|
|
global_data.streamRateRawSensorFusion = freq;
|
|
global_data.streamRatePosition = freq;
|
|
global_data.streamRateExtra1 = freq;
|
|
global_data.streamRateExtra2 = freq;
|
|
global_data.streamRateExtra3 = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_RAW_SENSORS:
|
|
global_data.streamRateRawSensors = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_EXTENDED_STATUS:
|
|
global_data.streamRateExtendedStatus = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_RC_CHANNELS:
|
|
global_data.streamRateRCChannels = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_RAW_CONTROLLER:
|
|
global_data.streamRateRawController = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
|
|
global_data.streamRateRawSensorFusion = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_POSITION:
|
|
global_data.streamRatePosition = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_EXTRA1:
|
|
global_data.streamRateExtra1 = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_EXTRA2:
|
|
global_data.streamRateExtra2 = freq;
|
|
break;
|
|
case MAV_DATA_STREAM_EXTRA3:
|
|
global_data.streamRateExtra3 = freq;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_ACTION:
|
|
{
|
|
// decode
|
|
mavlink_action_t packet;
|
|
mavlink_msg_action_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target,packet.target_component)) break;
|
|
|
|
// do action
|
|
gcs.send_text(SEVERITY_LOW,"action received");
|
|
switch(packet.action)
|
|
{
|
|
|
|
case MAV_ACTION_LAUNCH:
|
|
set_mode(TAKEOFF);
|
|
break;
|
|
|
|
case MAV_ACTION_RETURN:
|
|
set_mode(RTL);
|
|
break;
|
|
|
|
case MAV_ACTION_EMCY_LAND:
|
|
set_mode(LAND);
|
|
break;
|
|
|
|
case MAV_ACTION_HALT:
|
|
loiter_at_location();
|
|
break;
|
|
|
|
// can't implement due to APM configuration
|
|
// just setting to manual to be safe
|
|
case MAV_ACTION_MOTORS_START:
|
|
case MAV_ACTION_CONFIRM_KILL:
|
|
case MAV_ACTION_EMCY_KILL:
|
|
case MAV_ACTION_MOTORS_STOP:
|
|
case MAV_ACTION_SHUTDOWN:
|
|
set_mode(MANUAL);
|
|
break;
|
|
|
|
case MAV_ACTION_CONTINUE:
|
|
process_next_command();
|
|
break;
|
|
|
|
case MAV_ACTION_SET_MANUAL:
|
|
set_mode(MANUAL);
|
|
break;
|
|
|
|
case MAV_ACTION_SET_AUTO:
|
|
set_mode(AUTO);
|
|
break;
|
|
|
|
case MAV_ACTION_STORAGE_READ:
|
|
//read_EEPROM_startup();
|
|
//read_EEPROM_airstart_critical();
|
|
//read_command_index();
|
|
//read_EEPROM_flight_modes();
|
|
break;
|
|
|
|
case MAV_ACTION_STORAGE_WRITE:
|
|
//save_EEPROM_trims();
|
|
//save_EEPROM_waypoint_info();
|
|
//save_EEPROM_gains();
|
|
//save_command_index();
|
|
//save_pressure_data();
|
|
//save_EEPROM_radio_minmax();
|
|
//save_user_configs();
|
|
//save_EEPROM_flight_modes();
|
|
break;
|
|
|
|
case MAV_ACTION_CALIBRATE_RC: break;
|
|
trim_radio();
|
|
break;
|
|
|
|
case MAV_ACTION_CALIBRATE_GYRO:
|
|
case MAV_ACTION_CALIBRATE_MAG:
|
|
case MAV_ACTION_CALIBRATE_ACC:
|
|
case MAV_ACTION_CALIBRATE_PRESSURE:
|
|
case MAV_ACTION_REBOOT: // this is a rough interpretation
|
|
startup_IMU_ground();
|
|
break;
|
|
|
|
case MAV_ACTION_REC_START: break;
|
|
case MAV_ACTION_REC_PAUSE: break;
|
|
case MAV_ACTION_REC_STOP: break;
|
|
|
|
case MAV_ACTION_TAKEOFF:
|
|
set_mode(TAKEOFF);
|
|
break;
|
|
|
|
case MAV_ACTION_NAVIGATE:
|
|
set_mode(AUTO);
|
|
break;
|
|
|
|
case MAV_ACTION_LAND:
|
|
set_mode(LAND);
|
|
break;
|
|
|
|
case MAV_ACTION_LOITER:
|
|
set_mode(LOITER);
|
|
break;
|
|
|
|
default: break;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
|
|
{
|
|
//send_text(SEVERITY_LOW,"waypoint request list");
|
|
|
|
// decode
|
|
mavlink_waypoint_request_list_t packet;
|
|
mavlink_msg_waypoint_request_list_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// Start sending waypoints
|
|
mavlink_msg_waypoint_count_send(chan,msg->sysid,
|
|
msg->compid,get(PARAM_WP_TOTAL));
|
|
global_data.waypoint_timelast_send = millis();
|
|
global_data.waypoint_sending = true;
|
|
global_data.waypoint_receiving = false;
|
|
global_data.waypoint_dest_sysid = msg->sysid;
|
|
global_data.waypoint_dest_compid = msg->compid;
|
|
|
|
}
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
|
|
{
|
|
//send_text(SEVERITY_LOW,"waypoint request");
|
|
|
|
// Check if sending waypiont
|
|
if (!global_data.waypoint_sending) break;
|
|
|
|
// decode
|
|
mavlink_waypoint_request_t packet;
|
|
mavlink_msg_waypoint_request_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// send waypoint
|
|
tell_command = get_wp_with_index(packet.seq);
|
|
|
|
// set frame of waypoint
|
|
uint8_t frame = MAV_FRAME_GLOBAL; // reference frame
|
|
uint8_t action = MAV_ACTION_NAVIGATE; // action
|
|
uint8_t orbit_direction = 0; // clockwise(0), counter-clockwise(1)
|
|
float orbit = 0; // loiter radius
|
|
float param1 = 0, param2 = 0;
|
|
|
|
switch(tell_command.id)
|
|
{
|
|
|
|
case CMD_WAYPOINT: // navigate
|
|
action = MAV_ACTION_NAVIGATE; // action
|
|
break;
|
|
|
|
case CMD_LOITER_TIME: // loiter
|
|
orbit = get(PARAM_WP_RADIUS); // XXX setting loiter radius as waypoint acceptance radius
|
|
action = MAV_ACTION_LOITER; // action
|
|
param1 = get(PARAM_WP_RADIUS);
|
|
param2 = tell_command.p1*100; // loiter time
|
|
break;
|
|
|
|
case CMD_TAKEOFF: // takeoff
|
|
action = MAV_ACTION_TAKEOFF;
|
|
break;
|
|
|
|
case CMD_LAND: // land
|
|
action = MAV_ACTION_LAND;
|
|
break;
|
|
|
|
defaut:
|
|
gcs.send_text(SEVERITY_LOW,"command not handled");
|
|
break;
|
|
}
|
|
|
|
// time that the mav should loiter in milliseconds
|
|
uint8_t current = 0; // 1 (true), 0 (false)
|
|
if (packet.seq == get(PARAM_WP_INDEX)) current = 1;
|
|
float yaw_dir = 0; // yaw orientation in radians, 0 = north XXX: what does this do?
|
|
uint8_t autocontinue = 1; // 1 (true), 0 (false)
|
|
float x = tell_command.lng/1.0e7; // local (x), global (longitude)
|
|
float y = tell_command.lat/1.0e7; // local (y), global (latitude)
|
|
float z = tell_command.alt/1.0e2; // local (z), global (altitude)
|
|
// note XXX: documented x,y,z order does not match with gps raw
|
|
mavlink_msg_waypoint_send(chan,msg->sysid,
|
|
msg->compid,packet.seq,frame,action,
|
|
orbit,orbit_direction,param1,param2,current,x,y,z,yaw_dir,autocontinue);
|
|
|
|
// update last waypoint comm stamp
|
|
global_data.waypoint_timelast_send = millis();
|
|
}
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_ACK:
|
|
{
|
|
//send_text(SEVERITY_LOW,"waypoint ack");
|
|
|
|
// decode
|
|
mavlink_waypoint_ack_t packet;
|
|
mavlink_msg_waypoint_ack_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// check for error
|
|
uint8_t type = packet.type; // ok (0), error(1)
|
|
|
|
// turn off waypoint send
|
|
global_data.waypoint_sending = false;
|
|
}
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
|
{
|
|
//send_text(SEVERITY_LOW,"param request list");
|
|
|
|
// decode
|
|
mavlink_param_request_list_t packet;
|
|
mavlink_msg_param_request_list_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// Start sending parameters
|
|
global_data.parameter_i = 0;
|
|
}
|
|
break;
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
|
|
{
|
|
//send_text(SEVERITY_LOW,"waypoint clear all");
|
|
|
|
// decode
|
|
mavlink_waypoint_clear_all_t packet;
|
|
mavlink_msg_waypoint_clear_all_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// clear all waypoints
|
|
uint8_t type = 0; // ok (0), error(1)
|
|
set(PARAM_WP_TOTAL,0);
|
|
|
|
// send acknowledgement 3 times to makes sure it is received
|
|
for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
|
|
|
|
break;
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
|
|
{
|
|
//send_text(SEVERITY_LOW,"waypoint set current");
|
|
|
|
// decode
|
|
mavlink_waypoint_set_current_t packet;
|
|
mavlink_msg_waypoint_set_current_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// set current waypoint
|
|
set(PARAM_WP_INDEX,packet.seq);
|
|
{
|
|
Location temp; // XXX this is gross
|
|
temp = get_wp_with_index(packet.seq);
|
|
set_next_WP(&temp);
|
|
}
|
|
mavlink_msg_waypoint_current_send(chan,get(PARAM_WP_INDEX));
|
|
break;
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
|
|
{
|
|
//send_text(SEVERITY_LOW,"waypoint count");
|
|
|
|
// decode
|
|
mavlink_waypoint_count_t packet;
|
|
mavlink_msg_waypoint_count_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// start waypoint receiving
|
|
set(PARAM_WP_TOTAL,packet.count);
|
|
if (get(PARAM_WP_TOTAL) > MAX_WAYPOINTS)
|
|
set(PARAM_WP_TOTAL,MAX_WAYPOINTS);
|
|
global_data.waypoint_timelast_receive = millis();
|
|
global_data.waypoint_receiving = true;
|
|
global_data.waypoint_sending = false;
|
|
global_data.waypoint_request_i = 0;
|
|
break;
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT:
|
|
{
|
|
// Check if receiving waypiont
|
|
if (!global_data.waypoint_receiving) break;
|
|
|
|
// decode
|
|
mavlink_waypoint_t packet;
|
|
mavlink_msg_waypoint_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
|
|
|
// check if this is the requested waypoint
|
|
if (packet.seq != global_data.waypoint_request_i) break;
|
|
|
|
// store waypoint
|
|
uint8_t loadAction = 0; // 0 insert in list, 1 exec now
|
|
|
|
switch (packet.frame)
|
|
{
|
|
case MAV_FRAME_GLOBAL:
|
|
{
|
|
tell_command.lng = 1.0e7*packet.x;
|
|
tell_command.lat = 1.0e7*packet.y;
|
|
tell_command.alt = packet.z*1.0e2;
|
|
break;
|
|
}
|
|
|
|
case MAV_FRAME_LOCAL: // local (relative to home position)
|
|
{
|
|
tell_command.lng = 1.0e7*ToDeg(packet.x/
|
|
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lng;
|
|
tell_command.lat = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lat;
|
|
tell_command.alt = -packet.z*1.0e2 + home.alt;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// defaults
|
|
tell_command.id = CMD_BLANK;
|
|
|
|
switch (packet.action)
|
|
{
|
|
|
|
case MAV_ACTION_TAKEOFF:
|
|
{
|
|
tell_command.id = CMD_TAKEOFF;
|
|
break;
|
|
}
|
|
case MAV_ACTION_LAND:
|
|
{
|
|
tell_command.id = CMD_LAND;
|
|
break;
|
|
}
|
|
|
|
case MAV_ACTION_NAVIGATE:
|
|
{
|
|
tell_command.id = CMD_WAYPOINT;
|
|
break;
|
|
}
|
|
|
|
case MAV_ACTION_LOITER:
|
|
{
|
|
tell_command.id = CMD_LOITER_TIME;
|
|
tell_command.p1 = packet.param2/1.0e2;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// save waypoint
|
|
set_wp_with_index(tell_command, packet.seq);
|
|
|
|
// update waypoint receiving state machine
|
|
global_data.waypoint_timelast_receive = millis();
|
|
global_data.waypoint_request_i++;
|
|
|
|
if (global_data.waypoint_request_i == get(PARAM_WP_TOTAL))
|
|
{
|
|
gcs.send_text(SEVERITY_LOW,"flight plane received");
|
|
uint8_t type = 0; // ok (0), error(1)
|
|
mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
|
|
global_data.waypoint_receiving = false;
|
|
set(PARAM_WP_RADIUS,packet.param1); // XXX takes last waypoint radius for all
|
|
//save_EEPROM_waypoint_info();
|
|
}
|
|
break;
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_PARAM_SET:
|
|
{
|
|
// decode
|
|
mavlink_param_set_t packet;
|
|
mavlink_msg_param_set_decode(msg, &packet);
|
|
if (mavlink_check_target(packet.target_system,packet.target_component))
|
|
break;
|
|
|
|
// set parameter
|
|
const char * key = (const char*) packet.param_id;
|
|
|
|
// iterate known parameters
|
|
// XXX linear search; would be better to sort params & use a binary search
|
|
for (uint16_t i = 0; i < global_data.param_count; i++) {
|
|
|
|
// compare key with parameter name
|
|
if (!strcmp_P(key, getParamName(i))) {
|
|
|
|
// sanity-check the new parameter
|
|
if (!isnan(packet.param_value) && // not nan
|
|
!isinf(packet.param_value)) { // not inf
|
|
|
|
setParamAsFloat(i,packet.param_value);
|
|
|
|
// Report back new value
|
|
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
|
|
strcpy_P(param_name, getParamName(i)); /// XXX HACK
|
|
mavlink_msg_param_value_send(chan,
|
|
(int8_t*)param_name,
|
|
getParamAsFloat(i),
|
|
global_data.param_count,i);
|
|
// call load if required
|
|
if (i >= PARAM_RLL2SRV_P && i <= PARAM_RLL2SRV_IMAX) pidServoRoll.load_gains();
|
|
if (i >= PARAM_PTCH2SRV_P && i <= PARAM_PTCH2SRV_IMAX) pidServoPitch.load_gains();
|
|
if (i >= PARAM_YW2SRV_P && i <= PARAM_YW2SRV_IMAX) pidServoRudder.load_gains();
|
|
if (i >= PARAM_HDNG2RLL_P && i <= PARAM_HDNG2RLL_IMAX) pidNavRoll.load_gains();
|
|
if (i >= PARAM_ARSPD2PTCH_P && i <= PARAM_ARSPD2PTCH_IMAX) pidNavPitchAirspeed.load_gains();
|
|
if (i >= PARAM_ALT2PTCH_P && i <= PARAM_ALT2PTCH_IMAX) pidNavPitchAltitude.load_gains();
|
|
if (i >= PARAM_ENRGY2THR_P && i <= PARAM_ENRGY2THR_IMAX) pidTeThrottle.load_gains();
|
|
if (i >= PARAM_ALT2THR_P && i <= PARAM_ALT2THR_IMAX) pidAltitudeThrottle.load_gains();
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
} // end case
|
|
} // end switch
|
|
} // end handle mavlink
|
|
|
|
|
|
#endif
|
|
|