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

AntennaTracker: add support for PROXY_MODE

This commit is contained in:
Mike McCauley 2014-05-14 12:11:22 +10:00 committed by Andrew Tridgell
parent 43059fbff5
commit 7e3daacda0
5 changed files with 186 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -194,6 +194,10 @@ static RC_Channel channel_pitch(CH_2);
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
// If proxy_mode is enabled, uartC is connected to a remote vehicle,
// not gcs[1]
static GCS_MAVLINK proxy_vehicle;
// board specific config
static AP_BoardConfig BoardConfig;

173
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -528,14 +528,73 @@ GCS_MAVLINK::data_stream_send(void)
}
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
if (g.proxy_mode == true)
{
if (chan == proxy_vehicle.chan)
{
// From the remote vehicle.
// All messages from the remote are proxied to GCS
// 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);
}
}
}
}
// Else its from the GCS, and it might be for the remote and.or it might be for the tracker
// So we fall through to the below
}
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;
}
@ -543,24 +602,75 @@ 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;
}
@ -570,7 +680,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) break;
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;
@ -674,8 +791,14 @@ 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)) break;
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
@ -696,7 +819,14 @@ 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)) break;
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 = {};
@ -777,9 +907,15 @@ mission_failed:
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Also proxy it to 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);
}
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
tracking_manual_control(packet);
break;
}
@ -789,6 +925,11 @@ mission_failed:
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Also proxy it to 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);
}
tracking_update_position(packet);
break;
}
@ -798,6 +939,11 @@ mission_failed:
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Also proxy it to 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);
}
tracking_update_pressure(packet);
break;
}
@ -807,6 +953,11 @@ mission_failed:
// decode
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg, &packet);
if (g.proxy_mode == true && proxy_vehicle.initialised) {
// Also proxy it to 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);
}
if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
// we ignore base_mode as there is no sane way to map
@ -833,6 +984,11 @@ mission_failed:
#endif
default:
// Proxy all other messages to the remote
if (g.proxy_mode && 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;
} // end switch
@ -905,6 +1061,9 @@ static void gcs_update(void)
gcs[i].update(NULL);
}
}
// Also check for messages from the remote if we are in proxy mode
if (g.proxy_mode == true && proxy_vehicle.initialised)
proxy_vehicle.update();
}
static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)

2
Tools/AntennaTracker/Parameters.h
View File

@ -80,6 +80,7 @@ public:
k_param_BoardConfig,
k_param_gps,
k_param_scan_speed,
k_param_proxy_mode,
k_param_channel_yaw = 200,
k_param_channel_pitch,
@ -116,6 +117,7 @@ public:
AP_Float start_longitude;
AP_Float startup_delay;
AP_Int8 proxy_mode;
// Waypoints
//

8
Tools/AntennaTracker/Parameters.pde
View File

@ -115,6 +115,14 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 10
GSCALAR(startup_delay, "STARTUP_DELAY", 0),
// @Param: PROXY_MODE
// @DisplayName: Also act as a MAVLink proxy for a vehicle
// @Description: If true, the tracker will act as a MAVlink proxy for a remote vehicle, and will eavesdrop vehicle position reports.
// @Units: boolean
// @Increment: 1
// @Range: 0 1
GSCALAR(proxy_mode, "PROXY_MODE", 0),
// barometer ground calibration. The GND_ prefix is chosen for
// compatibility with previous releases of ArduPlane
// @Group: GND_

7
Tools/AntennaTracker/system.pde
View File

@ -35,7 +35,12 @@ static void init_tracker()
check_usb_mux();
// we have a 2nd serial port for telemetry
gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE);
hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD),
128, SERIAL1_BUFSIZE);
if (g.proxy_mode == true)
proxy_vehicle.init(hal.uartC);
else
gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE);
mavlink_system.sysid = g.sysid_this_mav;