Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: fixed for new MAVLink handling

This commit is contained in:
Andrew Tridgell 2014-03-19 09:21:19 +11:00 committed by Randy Mackay
parent 0aafef9f69
commit 851fb61901
3 changed files with 10 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -53,6 +53,7 @@
#include <AP_NavEKF.h>
#include <AP_Vehicle.h>
#include <AP_Mission.h>
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_Airspeed.h>

180
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -658,204 +658,35 @@ GCS_MAVLINK::send_message(enum ap_message id)
mavlink_send_message(chan,id, packet_drops);
}
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)
{
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;
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;
}
handle_request_data_stream(msg, true);
break;
}
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;
// 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();
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
// decode
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(
chan,
param_name,
value,
mav_var_type(p_type),
_count_parameters(),
packet.param_index);
handle_param_request_read(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
AP_Param *vp;
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
handle_param_set(msg, NULL);
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(
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;
} // end case
case MAVLINK_MSG_ID_HEARTBEAT:
{
if (msg->sysid != g.sysid_my_gcs) break;
@ -995,9 +826,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
struct Location tell_command = {};
// defaults
tell_command.id = packet.command;
switch (packet.frame)
{
case MAV_FRAME_MISSION:

21
Tools/AntennaTracker/system.pde
View File

@ -136,25 +136,18 @@ static struct Location get_home_eeprom()
// --------------------------------------------------------------------------------
if (g.command_total.get() == 0) {
memset(&temp, 0, sizeof(temp));
temp.id = CMD_BLANK;
}else{
// read WP position
mem = WP_START_BYTE;
temp.id = hal.storage->read_byte(mem);
mem++;
temp.options = hal.storage->read_byte(mem);
mem++;
temp.p1 = hal.storage->read_byte(mem);
mem++;
temp.alt = hal.storage->read_dword(mem);
mem += 4;
temp.lat = hal.storage->read_dword(mem);
mem += 4;
temp.lng = hal.storage->read_dword(mem);
}
@ -165,21 +158,15 @@ static void set_home_eeprom(struct Location temp)
{
uint16_t mem = WP_START_BYTE;
hal.storage->write_byte(mem, temp.id);
mem++;
hal.storage->write_byte(mem, temp.options);
mem++;
hal.storage->write_byte(mem, temp.p1);
mem++;
hal.storage->write_dword(mem, temp.alt);
mem += 4;
hal.storage->write_dword(mem, temp.lat);
mem += 4;
hal.storage->write_dword(mem, temp.lng);
// Now have a home location in EEPROM