zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AntennaTracker: use generic mavlink routing 

this replaces the special purpose routing in the AntennaTracker
master
Andrew Tridgell 10 years ago committed by Randy Mackay
parent
41a5f79046
commit
c94e934fdf
2 changed files with 29 additions and 142 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 168
      AntennaTracker/GCS_Mavlink.pde
  2. 3
      AntennaTracker/system.pde

168
AntennaTracker/GCS_Mavlink.pde
Unescape View File

@ -437,69 +437,41 @@ GCS_MAVLINK::data_stream_send(void) @@ -437,69 +437,41 @@ 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:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracking_update_pressure(packet);
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;
switch (msg->msgid) {
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracking_update_pressure(packet);
break;
}
}
}
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) @@ -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) @@ -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) @@ -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: @@ -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
Unescape View File

@ -28,6 +28,9 @@ static void init_tracker() @@ -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);

Write Preview
Loading…
Cancel
Save