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

AntennaTracker: use generic mavlink routing 

this replaces the special purpose routing in the AntennaTracker
This commit is contained in:
Andrew Tridgell 2014-12-10 14:32:30 +11:00 committed by Randy Mackay
parent 41a5f79046
commit c94e934fdf
2 changed files with 30 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
AntennaTracker/GCS_Mavlink.pde
View File

@ -437,12 +437,12 @@ GCS_MAVLINK::data_stream_send(void)
}
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
/*
We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and
MAVLINK_MSG_ID_SCALED_PRESSUREs
*/
void mavlink_snoop(const mavlink_message_t* msg)
{
// in proxy mode all messages from the vehicle are proxied to GCS
if (g.proxy_mode == true && chan == proxy_vehicle.chan) {
// We also eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and MAVLINK_MSG_ID_SCALED_PRESSUREs
switch (msg->msgid) {
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
@ -462,44 +462,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
}
// Proxy to all the GCS stations
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
mavlink_channel_t out_chan = (mavlink_channel_t)i;
// only forward if it would fit in the transmit buffer
if (comm_get_txspace(out_chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
_mavlink_resend_uart(out_chan, msg);
}
}
}
// no further processing of this messages
return;
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// If we are currently operating as a proxy for a remote,
// alas we have to look inside each packet to see if its for us or for the remote
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_request_data_stream_t packet;
mavlink_msg_request_data_stream_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_request_data_stream(msg, true);
break;
}
@ -507,92 +479,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_param_request_read_t packet;
mavlink_msg_param_request_read_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_param_request_read(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
// See if its for the remote
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
{
// Not for us, must be for the remote
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
break;
}
// Else its for us, the tracker
}
handle_param_set(msg, NULL);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// Heartbeats are always proxied to the remote and also handled locally
if (g.proxy_mode == true && proxy_vehicle.initialised)
{
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
if (msg->sysid != g.sysid_my_gcs) break;
break;
}
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// Its for the remote, proxy it
if (g.proxy_mode == true && proxy_vehicle.initialised) {
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
uint8_t result = MAV_RESULT_UNSUPPORTED;
@ -696,14 +606,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Its for the remote, proxy it
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
if (packet.start_index == 0)
{
// New home at wp index 0. Ask for it
@ -724,14 +626,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_item_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Its for the remote, proxy it
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
struct Location tell_command = {};
@ -814,16 +708,6 @@ mission_failed:
{
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
// if the packet is not for us, send onto the vehicle
if (mavlink_check_target(packet.target,0)) {
if (g.proxy_mode == true && proxy_vehicle.initialised) {
if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES)
_mavlink_resend_uart(proxy_vehicle.chan, msg);
}
break;
}
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
tracking_manual_control(packet);
break;
}

3
AntennaTracker/system.pde
View File

@ -28,6 +28,9 @@ static void init_tracker()
// init the GCS
gcs[0].init(hal.uartA);
// set up snooping on other mavlink destinations
gcs[0].set_snoop(mavlink_snoop);
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);