Plane: use new common MAVLink code

This commit is contained in:
Andrew Tridgell 2014-03-19 08:34:35 +11:00 committed by Randy Mackay
parent 965f6bd3bd
commit 0d39f354b0
3 changed files with 9 additions and 234 deletions

View File

@ -439,9 +439,6 @@ static struct {
uint32_t ch3_timer_ms; uint32_t ch3_timer_ms;
uint32_t last_valid_rc_ms; uint32_t last_valid_rc_ms;
// last RADIO status packet
uint32_t last_radio_status_remrssi_ms;
} failsafe; } failsafe;

View File

@ -839,24 +839,6 @@ static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id)
} }
} }
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
{
if (telemetry_delayed(chan)) {
return;
}
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
mavlink_send_message(chan, MSG_STATUSTEXT);
} else {
// send immediately
mavlink_msg_statustext_send(chan, severity, str);
}
}
/* /*
default stream rates to 1Hz default stream rates to 1Hz
*/ */
@ -1144,21 +1126,6 @@ GCS_MAVLINK::send_message(enum ap_message id)
mavlink_send_message(chan, id); mavlink_send_message(chan, id);
} }
void
GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str)
{
mavlink_statustext_t m;
uint8_t i;
for (i=0; i<sizeof(m.text); i++) {
m.text[i] = pgm_read_byte((const prog_char *)(str++));
if (m.text[i] == '\0') {
break;
}
}
if (i < sizeof(m.text)) m.text[i] = 0;
mavlink_send_text(chan, severity, (const char *)m.text);
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{ {
struct AP_Mission::Mission_Command cmd = {}; // general purpose mission command struct AP_Mission::Mission_Command cmd = {}; // general purpose mission command
@ -1167,54 +1134,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{ {
// decode handle_request_data_stream(msg, true);
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;
int16_t 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:
// note that we don't set STREAM_PARAMS - that is internal only
for (uint8_t i=0; i<STREAM_PARAMS; i++) {
streamRates[i].set_and_save_ifchanged(freq);
}
break;
case MAV_DATA_STREAM_RAW_SENSORS:
streamRates[STREAM_RAW_SENSORS].set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRates[STREAM_EXTENDED_STATUS].set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_RC_CHANNELS:
streamRates[STREAM_RC_CHANNELS].set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRates[STREAM_RAW_CONTROLLER].set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_POSITION:
streamRates[STREAM_POSITION].set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_EXTRA1:
streamRates[STREAM_EXTRA1].set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_EXTRA2:
streamRates[STREAM_EXTRA2].set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_EXTRA3:
streamRates[STREAM_EXTRA3].set_and_save_ifchanged(freq);
break;
}
break; break;
} }
@ -1426,77 +1346,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_MISSION_ACK: case MAVLINK_MSG_ID_MISSION_ACK:
{ {
// decode // nothing to do
mavlink_mission_ack_t packet;
mavlink_msg_mission_ack_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
break; break;
} }
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: case MAVLINK_MSG_ID_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;
// mark the firmware version in the tlog // mark the firmware version in the tlog
send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING)); send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING));
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION)); send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
#endif #endif
handle_param_request_list(msg);
// send system ID if we can
char sysid[40];
if (hal.util->get_system_id(sysid)) {
mavlink_send_text(chan, SEVERITY_LOW, sysid);
}
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
break; break;
} }
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{ {
// decode handle_param_request_read(msg);
mavlink_param_request_read_t packet;
mavlink_msg_param_request_read_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
enum ap_var_type p_type;
AP_Param *vp;
char param_name[AP_MAX_NAME_SIZE+1];
if (packet.param_index != -1) {
AP_Param::ParamToken token;
vp = AP_Param::find_by_index(packet.param_index, &p_type, &token);
if (vp == NULL) {
gcs_send_text_fmt(PSTR("Unknown parameter index %d"), packet.param_index);
break;
}
vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true);
param_name[AP_MAX_NAME_SIZE] = 0;
} else {
strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);
param_name[AP_MAX_NAME_SIZE] = 0;
vp = AP_Param::find(param_name, &p_type);
if (vp == NULL) {
gcs_send_text_fmt(PSTR("Unknown parameter %.16s"), packet.param_id);
break;
}
}
float value = vp->cast_to_float(p_type);
mavlink_msg_param_value_send_buf(
msg,
chan,
param_name,
value,
mav_var_type(p_type),
_count_parameters(),
packet.param_index);
break; break;
} }
@ -1757,75 +1625,9 @@ mission_failed:
case MAVLINK_MSG_ID_PARAM_SET: case MAVLINK_MSG_ID_PARAM_SET:
{ {
AP_Param *vp; handle_param_set(msg, &DataFlash);
enum ap_var_type 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
char key[AP_MAX_NAME_SIZE+1];
strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
key[AP_MAX_NAME_SIZE] = 0;
// find the requested parameter
vp = AP_Param::find(key, &var_type);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.
float rounding_addition = 0.01;
// handle variables with standard type IDs
if (var_type == AP_PARAM_FLOAT) {
((AP_Float *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_PARAM_INT32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -2147483648.0, 2147483647.0);
((AP_Int32 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save(v);
} else if (var_type == AP_PARAM_INT8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition;
float v = packet.param_value+rounding_addition;
v = constrain_float(v, -128, 127);
((AP_Int8 *)vp)->set_and_save(v);
} else {
// we don't support mavlink set on this parameter
break;
}
// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send_buf(
msg,
chan,
key,
vp->cast_to_float(var_type),
mav_var_type(var_type),
_count_parameters(),
-1); // XXX we don't actually know what its index is...
#if LOGGING_ENABLED == ENABLED
DataFlash.Log_Write_Parameter(key, vp->cast_to_float(var_type));
#endif
}
break; break;
} // end case }
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{ {
@ -1952,31 +1754,7 @@ mission_failed:
case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS: case MAVLINK_MSG_ID_RADIO_STATUS:
{ {
mavlink_radio_t packet; handle_radio_status(msg);
mavlink_msg_radio_decode(msg, &packet);
// record if the GCS has been receiving radio messages from
// the aircraft
if (packet.remrssi != 0) {
failsafe.last_radio_status_remrssi_ms = hal.scheduler->millis();
}
// use the state of the transmit buffer in the radio to
// control the stream rate, giving us adaptive software
// flow control
if (packet.txbuf < 20 && stream_slowdown < 100) {
// we are very low on space - slow down a lot
stream_slowdown += 3;
} else if (packet.txbuf < 50 && stream_slowdown < 100) {
// we are a bit low on space, slow down slightly
stream_slowdown += 1;
} else if (packet.txbuf > 95 && stream_slowdown > 10) {
// the buffer has plenty of space, speed up a lot
stream_slowdown -= 2;
} else if (packet.txbuf > 90 && stream_slowdown != 0) {
// the buffer has enough space, speed up a bit
stream_slowdown--;
}
break; break;
} }

View File

@ -420,8 +420,8 @@ static void check_long_failsafe()
(tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS); failsafe_long_on_event(FAILSAFE_GCS);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
failsafe.last_radio_status_remrssi_ms != 0 && gcs[0].last_radio_status_remrssi_ms != 0 &&
(tnow - failsafe.last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) { (tnow - gcs[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS); failsafe_long_on_event(FAILSAFE_GCS);
} }
} else { } else {