mirror of https://github.com/ArduPilot/ardupilot
809 lines
26 KiB
Plaintext
809 lines
26 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
|
|
|
|
#include "Mavlink_Common.h"
|
|
|
|
GCS_MAVLINK::GCS_MAVLINK() :
|
|
packet_drops(0)
|
|
{
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::init(BetterStream * port)
|
|
{
|
|
GCS_Class::init(port);
|
|
if (port == &Serial) { // to split hil vs gcs
|
|
mavlink_comm_0_port = port;
|
|
chan = MAVLINK_COMM_0;
|
|
}else{
|
|
mavlink_comm_1_port = port;
|
|
chan = MAVLINK_COMM_1;
|
|
}
|
|
}
|
|
|
|
void
|
|
GCS_MAVLINK::update(void)
|
|
{
|
|
// receive 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
|
|
_queued_send();
|
|
|
|
// 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);
|
|
send_message(MSG_GPS_STATUS);
|
|
send_message(MSG_CURRENT_WAYPOINT);
|
|
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
|
|
}
|
|
|
|
if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax))
|
|
send_message(MSG_LOCATION);
|
|
|
|
if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) {
|
|
// This is used for HIL. Do not change without discussing with HIL maintainers
|
|
send_message(MSG_SERVO_OUT);
|
|
}
|
|
|
|
if (freqLoopMatch(global_data.streamRateRCChannels,freqMin,freqMax)) {
|
|
send_message(MSG_RADIO_OUT);
|
|
send_message(MSG_RADIO_IN);
|
|
}
|
|
|
|
if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax)) // Use Extra 1 for AHRS info
|
|
send_message(MSG_ATTITUDE);
|
|
|
|
if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) // Use Extra 3 for additional HIL info
|
|
send_message(MSG_VFR_HUD);
|
|
|
|
if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax))
|
|
{
|
|
// Available datastream
|
|
}
|
|
}
|
|
|
|
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_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.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;
|
|
}
|
|
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
|
|
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:
|
|
do_hold_position();
|
|
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(ACRO);
|
|
break;
|
|
|
|
case MAV_ACTION_CONTINUE:
|
|
process_next_command();
|
|
break;
|
|
|
|
case MAV_ACTION_SET_MANUAL:
|
|
set_mode(ACRO);
|
|
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_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,g.waypoint_total + 1); // + home
|
|
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;
|
|
global_data.requested_interface = chan;
|
|
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 command = tell_command.id; // command
|
|
float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
|
|
|
|
param1 = tell_command.p1;
|
|
|
|
// time that the mav should loiter in milliseconds
|
|
uint8_t current = 0; // 1 (true), 0 (false)
|
|
if (packet.seq == g.waypoint_index) current = 1;
|
|
uint8_t autocontinue = 1; // 1 (true), 0 (false)
|
|
float x = 0, y = 0, z = 0;
|
|
|
|
if (command < MAV_CMD_NAV_LAST) {
|
|
// command needs scaling
|
|
x = tell_command.lat/1.0e7; // local (x), global (longitude)
|
|
y = tell_command.lng/1.0e7; // local (y), global (latitude)
|
|
z = tell_command.alt/1.0e2; // local (z), global (altitude)
|
|
}else{
|
|
// command is raw
|
|
x = tell_command.lat;
|
|
y = tell_command.lng;
|
|
z = tell_command.alt;
|
|
}
|
|
// TODO - Need to add translation for many of the commands > MAV_CMD_NAV_LAST
|
|
|
|
// 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,command,current,autocontinue,
|
|
param1,param2,param3,param4,x,y,z);
|
|
|
|
// 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 - next call to ::update will kick the first one out
|
|
|
|
_queued_parameter = AP_Var::first();
|
|
_queued_parameter_index = 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)
|
|
g.waypoint_total.set_and_save(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
|
|
g.waypoint_index.set_and_save(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, g.waypoint_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
|
|
if (packet.count > MAX_WAYPOINTS) {
|
|
packet.count = MAX_WAYPOINTS;
|
|
}
|
|
g.waypoint_total.set_and_save(packet.count - 1);
|
|
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
|
|
|
|
// defaults
|
|
tell_command.id = packet.command;
|
|
|
|
switch (packet.frame)
|
|
{
|
|
case MAV_FRAME_GLOBAL:
|
|
{
|
|
tell_command.p1 = packet.param1; // in as byte no conversion
|
|
//tell_command.p1 = packet.param1; // in as byte no conversion
|
|
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
|
|
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
|
|
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
|
|
break;
|
|
}
|
|
|
|
case MAV_FRAME_LOCAL: // local (relative to home position)
|
|
{
|
|
tell_command.lat = 1.0e7*ToDeg(packet.x/
|
|
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lng;
|
|
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lat;
|
|
tell_command.alt = -packet.z*1.0e2 + home.alt;
|
|
break;
|
|
}
|
|
|
|
case MAV_FRAME_MISSION:
|
|
{
|
|
if(tell_command.id >= MAV_CMD_NAV_LAST){
|
|
// these are raw values
|
|
tell_command.p1 = packet.param1; // in as byte no conversion
|
|
tell_command.lat = packet.x; // in as long no conversion
|
|
tell_command.lng = packet.y; // in as long no conversion
|
|
tell_command.alt = packet.z; // in as int no conversion
|
|
}else{
|
|
tell_command.p1 = packet.param1; // in as byte no conversion
|
|
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
|
|
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
|
|
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
|
|
}
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
// save waypoint - and prevent re-setting home
|
|
if (packet.seq != 0)
|
|
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 > g.waypoint_total)
|
|
{
|
|
uint8_t type = 0; // ok (0), error(1)
|
|
mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
|
|
send_text(SEVERITY_LOW,"flight plan received");
|
|
global_data.waypoint_receiving = false;
|
|
// XXX ignores waypoint radius for individual waypoints, can
|
|
// only set WP_RADIUS parameter
|
|
}
|
|
break;
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_PARAM_SET:
|
|
{
|
|
AP_Var *vp;
|
|
AP_Meta_class::Type_id var_type;
|
|
|
|
// 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;
|
|
|
|
// find the requested parameter
|
|
vp = AP_Var::find(key);
|
|
if ((NULL != vp) && // exists
|
|
!isnan(packet.param_value) && // not nan
|
|
!isinf(packet.param_value)) { // not inf
|
|
|
|
// fetch the variable type ID
|
|
var_type = vp->meta_type_id();
|
|
|
|
// handle variables with standard type IDs
|
|
if (var_type == AP_Var::k_typeid_float) {
|
|
((AP_Float *)vp)->set(packet.param_value);
|
|
|
|
} else if (var_type == AP_Var::k_typeid_float16) {
|
|
((AP_Float16 *)vp)->set(packet.param_value);
|
|
|
|
} else if (var_type == AP_Var::k_typeid_int32) {
|
|
((AP_Int32 *)vp)->set(packet.param_value);
|
|
|
|
} else if (var_type == AP_Var::k_typeid_int16) {
|
|
((AP_Int16 *)vp)->set(packet.param_value);
|
|
|
|
} else if (var_type == AP_Var::k_typeid_int8) {
|
|
((AP_Int8 *)vp)->set(packet.param_value);
|
|
}
|
|
}
|
|
|
|
// Report back new value
|
|
char param_name[ONBOARD_PARAM_NAME_LENGTH]; // XXX HACK - need something to return a char *
|
|
vp->copy_name(param_name, sizeof(param_name));
|
|
mavlink_msg_param_value_send(chan,
|
|
(int8_t*)param_name,
|
|
packet.param_value,
|
|
_count_parameters(),
|
|
-1); // XXX we don't actually know what its index is...
|
|
|
|
break;
|
|
} // end case
|
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED
|
|
// This is used both as a sensor and to pass the location
|
|
// in HIL_ATTITUDE mode.
|
|
case MAVLINK_MSG_ID_GPS_RAW:
|
|
{
|
|
// decode
|
|
mavlink_gps_raw_t packet;
|
|
mavlink_msg_gps_raw_decode(msg, &packet);
|
|
|
|
// set gps hil sensor
|
|
g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
|
|
packet.v,packet.hdg,0,0);
|
|
break;
|
|
}
|
|
|
|
// Is this resolved? - MAVLink protocol change.....
|
|
case MAVLINK_MSG_ID_VFR_HUD:
|
|
{
|
|
// decode
|
|
mavlink_vfr_hud_t packet;
|
|
mavlink_msg_vfr_hud_decode(msg, &packet);
|
|
|
|
// set airspeed
|
|
airspeed = 100*packet.airspeed;
|
|
break;
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
|
|
{
|
|
// allow override of RC channel values for HIL
|
|
mavlink_rc_channels_raw_t packet;
|
|
int16_t v[8];
|
|
mavlink_msg_rc_channels_raw_decode(msg, &packet);
|
|
v[0] = packet.chan1_raw;
|
|
v[1] = packet.chan2_raw;
|
|
v[2] = packet.chan3_raw;
|
|
v[3] = packet.chan4_raw;
|
|
v[4] = packet.chan5_raw;
|
|
v[5] = packet.chan6_raw;
|
|
v[6] = packet.chan7_raw;
|
|
v[7] = packet.chan8_raw;
|
|
APM_RC.setHIL(v);
|
|
break;
|
|
}
|
|
#endif
|
|
#if HIL_MODE == HIL_MODE_ATTITUDE
|
|
case MAVLINK_MSG_ID_ATTITUDE:
|
|
{
|
|
// decode
|
|
mavlink_attitude_t packet;
|
|
mavlink_msg_attitude_decode(msg, &packet);
|
|
|
|
// set dcm hil sensor
|
|
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
|
|
packet.pitchspeed,packet.yawspeed);
|
|
break;
|
|
}
|
|
#endif
|
|
#if HIL_MODE == HIL_MODE_SENSORS
|
|
|
|
case MAVLINK_MSG_ID_RAW_IMU:
|
|
{
|
|
// decode
|
|
mavlink_raw_imu_t packet;
|
|
mavlink_msg_raw_imu_decode(msg, &packet);
|
|
|
|
// set imu hil sensors
|
|
// TODO: check scaling for temp/absPress
|
|
float temp = 70;
|
|
float absPress = 1;
|
|
// Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
|
|
// Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
|
|
|
|
// rad/sec
|
|
Vector3f gyros;
|
|
gyros.x = (float)packet.xgyro / 1000.0;
|
|
gyros.y = (float)packet.ygyro / 1000.0;
|
|
gyros.z = (float)packet.zgyro / 1000.0;
|
|
// m/s/s
|
|
Vector3f accels;
|
|
accels.x = (float)packet.xacc / 1000.0;
|
|
accels.y = (float)packet.yacc / 1000.0;
|
|
accels.z = (float)packet.zacc / 1000.0;
|
|
|
|
imu.set_gyro(gyros);
|
|
|
|
imu.set_accel(accels);
|
|
|
|
compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
|
|
break;
|
|
}
|
|
|
|
case MAVLINK_MSG_ID_RAW_PRESSURE:
|
|
{
|
|
// decode
|
|
mavlink_raw_pressure_t packet;
|
|
mavlink_msg_raw_pressure_decode(msg, &packet);
|
|
|
|
// set pressure hil sensor
|
|
// TODO: check scaling
|
|
float temp = 70;
|
|
barometer.setHIL(temp,packet.press_diff1);
|
|
break;
|
|
}
|
|
#endif // HIL_MODE
|
|
} // end switch
|
|
} // end handle mavlink
|
|
|
|
uint16_t
|
|
GCS_MAVLINK::_count_parameters()
|
|
{
|
|
// if we haven't cached the parameter count yet...
|
|
if (0 == _parameter_count) {
|
|
AP_Var *vp;
|
|
|
|
vp = AP_Var::first();
|
|
do {
|
|
// if a parameter responds to cast_to_float then we are going to be able to report it
|
|
if (!isnan(vp->cast_to_float())) {
|
|
_parameter_count++;
|
|
}
|
|
} while (NULL != (vp = vp->next()));
|
|
}
|
|
return _parameter_count;
|
|
}
|
|
|
|
AP_Var *
|
|
GCS_MAVLINK::_find_parameter(uint16_t index)
|
|
{
|
|
AP_Var *vp;
|
|
|
|
vp = AP_Var::first();
|
|
while (NULL != vp) {
|
|
|
|
// if the parameter is reportable
|
|
if (!(isnan(vp->cast_to_float()))) {
|
|
// if we have counted down to the index we want
|
|
if (0 == index) {
|
|
// return the parameter
|
|
return vp;
|
|
}
|
|
// count off this parameter, as it is reportable but not
|
|
// the one we want
|
|
index--;
|
|
}
|
|
// and move to the next parameter
|
|
vp = vp->next();
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/**
|
|
* @brief Send low-priority messages at a maximum rate of xx Hertz
|
|
*
|
|
* This function sends messages at a lower rate to not exceed the wireless
|
|
* bandwidth. It sends one message each time it is called until the buffer is empty.
|
|
* Call this function with xx Hertz to increase/decrease the bandwidth.
|
|
*/
|
|
void
|
|
GCS_MAVLINK::_queued_send()
|
|
{
|
|
|
|
// Check to see if we are sending parameters
|
|
while (NULL != _queued_parameter) {
|
|
AP_Var *vp;
|
|
float value;
|
|
|
|
|
|
|
|
// copy the current parameter and prepare to move to the next
|
|
vp = _queued_parameter;
|
|
_queued_parameter = _queued_parameter->next();
|
|
|
|
|
|
|
|
// if the parameter can be cast to float, report it here and break out of the loop
|
|
value = vp->cast_to_float();
|
|
if (!isnan(value)) {
|
|
|
|
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
|
|
vp->copy_name(param_name, sizeof(param_name));
|
|
|
|
mavlink_msg_param_value_send(chan,
|
|
(int8_t*)param_name,
|
|
value,
|
|
_count_parameters(),
|
|
_queued_parameter_index);
|
|
|
|
_queued_parameter_index++;
|
|
break;
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
|
|
mavdelay++;
|
|
|
|
// request waypoints one by one
|
|
// XXX note that this is pan-interface
|
|
if (global_data.waypoint_receiving && global_data.requested_interface == chan &&
|
|
global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz
|
|
{
|
|
mavlink_msg_waypoint_request_send(chan,
|
|
global_data.waypoint_dest_sysid,
|
|
global_data.waypoint_dest_compid ,global_data.waypoint_request_i);
|
|
mavdelay = 0;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|