diff --git a/AntennaTracker/GCS_Mavlink.pde b/AntennaTracker/GCS_Mavlink.pde index 5c0c88d9d5..57561265a0 100644 --- a/AntennaTracker/GCS_Mavlink.pde +++ b/AntennaTracker/GCS_Mavlink.pde @@ -437,69 +437,41 @@ GCS_MAVLINK::data_stream_send(void) } } +/* + We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and + MAVLINK_MSG_ID_SCALED_PRESSUREs +*/ +void mavlink_snoop(const mavlink_message_t* msg) +{ + 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) { - // 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 ((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) { // 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; } diff --git a/AntennaTracker/system.pde b/AntennaTracker/system.pde index 2382e57ef9..863d106f50 100644 --- a/AntennaTracker/system.pde +++ b/AntennaTracker/system.pde @@ -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);