diff --git a/Tools/AntennaTracker/APM_Config.h b/Tools/AntennaTracker/APM_Config.h
new file mode 100644
index 0000000000..fe01a4d2b3
--- /dev/null
+++ b/Tools/AntennaTracker/APM_Config.h
@@ -0,0 +1,5 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// This file is just a placeholder for your configuration file. If
+// you wish to change any of the setup parameters from their default
+// values, place the appropriate #define statements here.
diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde
new file mode 100644
index 0000000000..f1b67fc1a5
--- /dev/null
+++ b/Tools/AntennaTracker/AntennaTracker.pde
@@ -0,0 +1,271 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#define THISFIRMWARE "AntennaTracker V0.1"
+/*
+ Lead developers: Matthew Ridley and Andrew Tridgell
+
+ Please contribute your ideas! See http://dev.ardupilot.com for details
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+// Header includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include // ArduPilot GPS library
+#include // ArduPilot barometer library
+#include // ArduPilot Mega Magnetometer Library
+#include // ArduPilot Mega Vector/Matrix math Library
+#include // ArduPilot Mega Analog to Digital Converter Library
+#include
+#include // Inertial Sensor Library
+#include // ArduPilot Mega DCM Library
+#include // Filter library
+#include // APM FIFO Buffer
+#include
+
+#include // MAVLink GCS definitions
+#include // ArduPilot Mega Declination Helper Library
+#include
+#include
+#include
+#include // main loop scheduler
+
+#include
+#include // Notify library
+#include // Battery monitor library
+#include
+#include
+
+// Configuration
+#include "config.h"
+
+// Local modules
+#include "defines.h"
+
+#include "Parameters.h"
+#include "GCS.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+AP_HAL::BetterStream* cliSerial;
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+////////////////////////////////////////////////////////////////////////////////
+// the rate we run the main loop at
+////////////////////////////////////////////////////////////////////////////////
+static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
+
+////////////////////////////////////////////////////////////////////////////////
+// Parameters
+////////////////////////////////////////////////////////////////////////////////
+//
+// Global parameters are all contained within the 'g' class.
+//
+static Parameters g;
+
+// main loop scheduler
+static AP_Scheduler scheduler;
+
+// notification object for LEDs, buzzers etc
+static AP_Notify notify;
+
+// tracking status for MAVLink
+static struct {
+ float bearing;
+ float distance;
+ float pitch;
+} nav_status;
+
+
+////////////////////////////////////////////////////////////////////////////////
+// prototypes
+void gcs_send_text_fmt(const prog_char_t *fmt, ...);
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Sensors
+////////////////////////////////////////////////////////////////////////////////
+// All GPS access should be through this pointer.
+static GPS *g_gps;
+
+#if CONFIG_BARO == AP_BARO_BMP085
+static AP_Baro_BMP085 barometer;
+#elif CONFIG_BARO == AP_BARO_PX4
+static AP_Baro_PX4 barometer;
+#elif CONFIG_BARO == AP_BARO_HIL
+static AP_Baro_HIL barometer;
+#elif CONFIG_BARO == AP_BARO_MS5611
+ #if CONFIG_MS5611_SERIAL == AP_BARO_MS5611_SPI
+ static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
+ #elif CONFIG_MS5611_SERIAL == AP_BARO_MS5611_I2C
+ static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
+ #else
+ #error Unrecognized CONFIG_MS5611_SERIAL setting.
+ #endif
+#else
+ #error Unrecognized CONFIG_BARO setting
+#endif
+
+#if CONFIG_COMPASS == AP_COMPASS_PX4
+static AP_Compass_PX4 compass;
+#elif CONFIG_COMPASS == AP_COMPASS_HMC5843
+static AP_Compass_HMC5843 compass;
+#elif CONFIG_COMPASS == AP_COMPASS_HIL
+static AP_Compass_HIL compass;
+#else
+ #error Unrecognized CONFIG_COMPASS setting
+#endif
+
+// GPS selection
+AP_GPS_Auto g_gps_driver(&g_gps);
+
+#if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
+AP_InertialSensor_MPU6000 ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_PX4
+AP_InertialSensor_PX4 ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_HIL
+AP_InertialSensor_HIL ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
+AP_InertialSensor_Oilpan ins( &apm1_adc );
+#elif CONFIG_INS_TYPE == CONFIG_INS_FLYMAPLE
+AP_InertialSensor_Flymaple ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_L3G4200D
+AP_InertialSensor_L3G4200D ins;
+#else
+ #error Unrecognised CONFIG_INS_TYPE setting.
+#endif // CONFIG_INS_TYPE
+
+AP_AHRS_DCM ahrs(&ins, g_gps);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+SITL sitl;
+#endif
+
+/**
+ antenna control channels
+ */
+static RC_Channel channel_yaw(CH_1);
+static RC_Channel channel_pitch(CH_2);
+
+////////////////////////////////////////////////////////////////////////////////
+// GCS selection
+////////////////////////////////////////////////////////////////////////////////
+static GCS_MAVLINK gcs0;
+static GCS_MAVLINK gcs3;
+
+/*
+ scheduler table - all regular tasks apart from the fast_loop()
+ should be listed here, along with how often they should be called
+ (in 20ms units) and the maximum time they are expected to take (in
+ microseconds)
+ */
+static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
+ { update_ahrs, 1, 1000 },
+ { update_tracking, 1, 1000 },
+ { update_GPS, 5, 4000 },
+ { update_compass, 5, 1500 },
+ { update_barometer, 5, 1500 },
+ { update_tracking, 1, 1000 },
+ { gcs_update, 1, 1700 },
+ { gcs_data_stream_send, 1, 3000 },
+ { compass_accumulate, 1, 1500 },
+ { barometer_accumulate, 1, 900 },
+ { update_notify, 1, 100 },
+ { one_second_loop, 50, 3900 }
+};
+
+// setup the var_info table
+AP_Param param_loader(var_info, EEPROM_MAX_ADDR);
+
+/**
+ setup the sketch - called once on startup
+ */
+void setup()
+{
+ // this needs to be the first call, as it fills memory with
+ // sentinel values
+ memcheck_init();
+
+ cliSerial = hal.console;
+
+ // load the default values of variables listed in var_info[]
+ AP_Param::setup_sketch_defaults();
+
+ // arduplane does not use arming nor pre-arm checks
+ AP_Notify::flags.armed = true;
+ AP_Notify::flags.pre_arm_check = true;
+ AP_Notify::flags.failsafe_battery = false;
+
+ notify.init();
+ init_tracker();
+
+ // initialise the main loop scheduler
+ scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
+}
+
+/**
+ loop() is called continuously
+ */
+void loop()
+{
+ // wait for an INS sample
+ if (!ins.wait_for_sample(1000)) {
+ return;
+ }
+
+ // tell the scheduler one tick has passed
+ scheduler.tick();
+
+ scheduler.run(19900UL);
+}
+
+static void one_second_loop()
+{
+ // send a heartbeat
+ gcs_send_message(MSG_HEARTBEAT);
+
+ // make it possible to change orientation at runtime
+ ahrs.set_orientation();
+
+ // sync MAVLink system ID
+ mavlink_system.sysid = g.sysid_this_mav;
+
+ static uint8_t counter;
+ counter++;
+
+ if (counter >= 60) {
+ if(g.compass_enabled) {
+ compass.save_offsets();
+ }
+ counter = 0;
+ }
+}
+
+AP_HAL_MAIN();
diff --git a/Tools/AntennaTracker/GCS.h b/Tools/AntennaTracker/GCS.h
new file mode 100644
index 0000000000..d0f34c737f
--- /dev/null
+++ b/Tools/AntennaTracker/GCS.h
@@ -0,0 +1,187 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file GCS.h
+/// @brief Interface definition for the various Ground Control System
+// protocols.
+
+#ifndef __GCS_H
+#define __GCS_H
+
+#include
+#include
+#include
+#include
+
+///
+/// @class GCS
+/// @brief Class describing the interface between the APM code
+/// proper and the GCS implementation.
+///
+/// GCS' are currently implemented inside the sketch and as such have
+/// access to all global state. The sketch should not, however, call GCS
+/// internal functions - all calls to the GCS should be routed through
+/// this interface (or functions explicitly exposed by a subclass).
+///
+class GCS_Class
+{
+public:
+
+ /// Startup initialisation.
+ ///
+ /// This routine performs any one-off initialisation required before
+ /// GCS messages are exchanged.
+ ///
+ /// @note The stream is expected to be set up and configured for the
+ /// correct bitrate before ::init is called.
+ ///
+ /// @note The stream is currently BetterStream so that we can use the _P
+ /// methods; this may change if Arduino adds them to Print.
+ ///
+ /// @param port The stream over which messages are exchanged.
+ ///
+ void init(AP_HAL::UARTDriver *port) {
+ _port = port;
+ initialised = true;
+ }
+
+ /// Update GCS state.
+ ///
+ /// This may involve checking for received bytes on the stream,
+ /// or sending additional periodic messages.
+ void update(void) {
+ }
+
+ /// Send a message with a single numeric parameter.
+ ///
+ /// This may be a standalone message, or the GCS driver may
+ /// have its own way of locating additional parameters to send.
+ ///
+ /// @param id ID of the message to send.
+ /// @param param Explicit message parameter.
+ ///
+ void send_message(enum ap_message id) {
+ }
+
+ /// Send a text message.
+ ///
+ /// @param severity A value describing the importance of the message.
+ /// @param str The text to be sent.
+ ///
+ void send_text(gcs_severity severity, const char *str) {
+ }
+
+ /// Send a text message with a PSTR()
+ ///
+ /// @param severity A value describing the importance of the message.
+ /// @param str The text to be sent.
+ ///
+ void send_text_P(gcs_severity severity, const prog_char_t *str) {
+ }
+
+ // send streams which match frequency range
+ void data_stream_send(void);
+
+ // set to true if this GCS link is active
+ bool initialised;
+
+protected:
+ /// The stream we are communicating over
+ AP_HAL::UARTDriver *_port;
+};
+
+//
+// GCS class definitions.
+//
+// These are here so that we can declare the GCS object early in the sketch
+// and then reference it statically rather than via a pointer.
+//
+
+///
+/// @class GCS_MAVLINK
+/// @brief The mavlink protocol for qgroundcontrol
+///
+class GCS_MAVLINK : public GCS_Class
+{
+public:
+ GCS_MAVLINK();
+ void update(void);
+ void init(AP_HAL::UARTDriver *port);
+ void send_message(enum ap_message id);
+ void send_text(gcs_severity severity, const char *str);
+ void send_text_P(gcs_severity severity, const prog_char_t *str);
+ void data_stream_send(void);
+ void queued_param_send();
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+ // NOTE! The streams enum below and the
+ // set of AP_Int16 stream rates _must_ be
+ // kept in the same order
+ enum streams {STREAM_RAW_SENSORS,
+ STREAM_EXTENDED_STATUS,
+ STREAM_RC_CHANNELS,
+ STREAM_RAW_CONTROLLER,
+ STREAM_POSITION,
+ STREAM_EXTRA1,
+ STREAM_EXTRA2,
+ STREAM_EXTRA3,
+ STREAM_PARAMS,
+ NUM_STREAMS};
+
+ // see if we should send a stream now. Called at 50Hz
+ bool stream_trigger(enum streams stream_num);
+
+ // this costs us 51 bytes per instance, but means that low priority
+ // messages don't block the CPU
+ mavlink_statustext_t pending_status;
+
+ // call to reset the timeout window for entering the cli
+ void reset_cli_timeout();
+private:
+ void handleMessage(mavlink_message_t * msg);
+
+ /// Perform queued sending operations
+ ///
+ AP_Param * _queued_parameter; ///< next parameter to
+ // be sent in queue
+ enum ap_var_type _queued_parameter_type; ///< type of the next
+ // parameter
+ AP_Param::ParamToken _queued_parameter_token; ///AP_Param token for
+ // next() call
+ uint16_t _queued_parameter_index; ///< next queued
+ // parameter's index
+ uint16_t _queued_parameter_count; ///< saved count of
+ // parameters for
+ // queued send
+ uint32_t _queued_parameter_send_time_ms;
+
+ /// Count the number of reportable parameters.
+ ///
+ /// Not all parameters can be reported via MAVlink. We count the number
+ // that are
+ /// so that we can report to a GCS the number of parameters it should
+ // expect when it
+ /// requests the full set.
+ ///
+ /// @return The number of reportable parameters.
+ ///
+ uint16_t _count_parameters(); ///< count reportable
+ // parameters
+
+ uint16_t _parameter_count; ///< cache of reportable
+ // parameters
+
+ mavlink_channel_t chan;
+ uint16_t packet_drops;
+
+ // saveable rate of each stream
+ AP_Int16 streamRates[NUM_STREAMS];
+
+ // number of 50Hz ticks until we next send this stream
+ uint8_t stream_ticks[NUM_STREAMS];
+
+ // number of extra ticks to add to slow things down for the radio
+ uint8_t stream_slowdown;
+};
+
+#endif // __GCS_H
diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde
new file mode 100644
index 0000000000..9d16055c11
--- /dev/null
+++ b/Tools/AntennaTracker/GCS_Mavlink.pde
@@ -0,0 +1,981 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
+#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS)
+
+// use this to prevent recursion during sensor init
+static bool in_mavlink_delay;
+
+// true when we have received at least 1 MAVLink packet
+static bool mavlink_active;
+
+// check if a message will fit in the payload space available
+#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false
+
+/*
+ * !!NOTE!!
+ *
+ * the use of NOINLINE separate functions for each message type avoids
+ * a compiler bug in gcc that would cause it to use far more stack
+ * space than is needed. Without the NOINLINE we use the sum of the
+ * stack needed for each message type. Please be careful to follow the
+ * pattern below when adding any new messages
+ */
+
+static NOINLINE void send_heartbeat(mavlink_channel_t chan)
+{
+ mavlink_msg_heartbeat_send(
+ chan,
+ MAV_TYPE_ANTENNA_TRACKER,
+ MAV_AUTOPILOT_ARDUPILOTMEGA,
+ 0,
+ 0,
+ 0);
+}
+
+static NOINLINE void send_attitude(mavlink_channel_t chan)
+{
+ Vector3f omega = ahrs.get_gyro();
+ mavlink_msg_attitude_send(
+ chan,
+ hal.scheduler->millis(),
+ ahrs.roll,
+ ahrs.pitch,
+ ahrs.yaw,
+ omega.x,
+ omega.y,
+ omega.z);
+}
+
+
+static void NOINLINE send_location(mavlink_channel_t chan)
+{
+ uint32_t fix_time;
+ if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
+ fix_time = g_gps->last_fix_time;
+ } else {
+ fix_time = hal.scheduler->millis();
+ }
+
+ Location loc;
+ ahrs.get_position(loc);
+
+ mavlink_msg_global_position_int_send(
+ chan,
+ fix_time,
+ loc.lat, // in 1E7 degrees
+ loc.lng, // in 1E7 degrees
+ g_gps->altitude_cm * 10, // millimeters above sea level
+ 0,
+ g_gps->velocity_north() * 100, // X speed cm/s (+ve North)
+ g_gps->velocity_east() * 100, // Y speed cm/s (+ve East)
+ g_gps->velocity_down() * -100, // Z speed cm/s (+ve up)
+ ahrs.yaw_sensor);
+}
+
+static void NOINLINE send_gps_raw(mavlink_channel_t chan)
+{
+ mavlink_msg_gps_raw_int_send(
+ chan,
+ g_gps->last_fix_time*(uint64_t)1000,
+ g_gps->status(),
+ g_gps->latitude, // in 1E7 degrees
+ g_gps->longitude, // in 1E7 degrees
+ g_gps->altitude_cm * 10, // in mm
+ g_gps->hdop,
+ 65535,
+ g_gps->ground_speed_cm, // cm/s
+ g_gps->ground_course_cd, // 1/100 degrees,
+ g_gps->num_sats);
+}
+
+static void NOINLINE send_radio_out(mavlink_channel_t chan)
+{
+ mavlink_msg_servo_output_raw_send(
+ chan,
+ hal.scheduler->micros(),
+ 0, // port
+ hal.rcout->read(0),
+ hal.rcout->read(1),
+ hal.rcout->read(2),
+ hal.rcout->read(3),
+ hal.rcout->read(4),
+ hal.rcout->read(5),
+ hal.rcout->read(6),
+ hal.rcout->read(7));
+}
+
+static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
+{
+ Vector3f accel = ins.get_accel();
+ Vector3f gyro = ins.get_gyro();
+
+ mavlink_msg_raw_imu_send(
+ chan,
+ hal.scheduler->micros(),
+ accel.x * 1000.0 / GRAVITY_MSS,
+ accel.y * 1000.0 / GRAVITY_MSS,
+ accel.z * 1000.0 / GRAVITY_MSS,
+ gyro.x * 1000.0,
+ gyro.y * 1000.0,
+ gyro.z * 1000.0,
+ compass.mag_x,
+ compass.mag_y,
+ compass.mag_z);
+}
+
+static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
+{
+ float pressure = barometer.get_pressure();
+ mavlink_msg_scaled_pressure_send(
+ chan,
+ hal.scheduler->millis(),
+ pressure*0.01f, // hectopascal
+ (pressure - barometer.get_ground_pressure())*0.01f, // hectopascal
+ barometer.get_temperature()*100); // 0.01 degrees C
+}
+
+static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
+{
+ // run this message at a much lower rate - otherwise it
+ // pointlessly wastes quite a lot of bandwidth
+ static uint8_t counter;
+ if (counter++ < 10) {
+ return;
+ }
+ counter = 0;
+
+ Vector3f mag_offsets = compass.get_offsets();
+ Vector3f accel_offsets = ins.get_accel_offsets();
+ Vector3f gyro_offsets = ins.get_gyro_offsets();
+
+ mavlink_msg_sensor_offsets_send(chan,
+ mag_offsets.x,
+ mag_offsets.y,
+ mag_offsets.z,
+ compass.get_declination(),
+ barometer.get_pressure(),
+ barometer.get_temperature()*100,
+ gyro_offsets.x,
+ gyro_offsets.y,
+ gyro_offsets.z,
+ accel_offsets.x,
+ accel_offsets.y,
+ accel_offsets.z);
+}
+
+static void NOINLINE send_ahrs(mavlink_channel_t chan)
+{
+ Vector3f omega_I = ahrs.get_gyro_drift();
+ mavlink_msg_ahrs_send(
+ chan,
+ omega_I.x,
+ omega_I.y,
+ omega_I.z,
+ 0,
+ 0,
+ ahrs.get_error_rp(),
+ ahrs.get_error_yaw());
+}
+
+static void NOINLINE send_hwstatus(mavlink_channel_t chan)
+{
+ mavlink_msg_hwstatus_send(
+ chan,
+ 0,
+ hal.i2c->lockup_count());
+}
+
+static void NOINLINE send_statustext(mavlink_channel_t chan)
+{
+ mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
+ mavlink_msg_statustext_send(
+ chan,
+ s->severity,
+ s->text);
+}
+
+static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
+{
+ mavlink_msg_nav_controller_output_send(
+ chan,
+ 0,
+ nav_status.pitch,
+ nav_status.bearing,
+ nav_status.bearing,
+ nav_status.distance,
+ 0,
+ 0,
+ 0);
+}
+
+
+// try to send a message, return false if it won't fit in the serial tx buffer
+static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
+{
+ int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+ switch (id) {
+ case MSG_HEARTBEAT:
+ CHECK_PAYLOAD_SIZE(HEARTBEAT);
+ send_heartbeat(chan);
+ return true;
+
+ case MSG_ATTITUDE:
+ CHECK_PAYLOAD_SIZE(ATTITUDE);
+ send_attitude(chan);
+ break;
+
+ case MSG_LOCATION:
+ CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
+ send_location(chan);
+ break;
+
+ case MSG_NAV_CONTROLLER_OUTPUT:
+ CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
+ send_nav_controller_output(chan);
+ break;
+
+ case MSG_GPS_RAW:
+ CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
+ send_gps_raw(chan);
+ break;
+
+ case MSG_RADIO_OUT:
+ CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
+ send_radio_out(chan);
+ break;
+
+ case MSG_RAW_IMU1:
+ CHECK_PAYLOAD_SIZE(RAW_IMU);
+ send_raw_imu1(chan);
+ break;
+
+ case MSG_RAW_IMU2:
+ CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
+ send_raw_imu2(chan);
+ break;
+
+ case MSG_RAW_IMU3:
+ CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
+ send_raw_imu3(chan);
+ break;
+
+ case MSG_NEXT_PARAM:
+ CHECK_PAYLOAD_SIZE(PARAM_VALUE);
+ if (chan == MAVLINK_COMM_0) {
+ gcs0.queued_param_send();
+ } else if (gcs3.initialised) {
+ gcs3.queued_param_send();
+ }
+ break;
+
+ case MSG_STATUSTEXT:
+ CHECK_PAYLOAD_SIZE(STATUSTEXT);
+ send_statustext(chan);
+ break;
+
+ case MSG_AHRS:
+ CHECK_PAYLOAD_SIZE(AHRS);
+ send_ahrs(chan);
+ break;
+
+ case MSG_HWSTATUS:
+ CHECK_PAYLOAD_SIZE(HWSTATUS);
+ send_hwstatus(chan);
+ break;
+
+ case MSG_SERVO_OUT:
+ case MSG_EXTENDED_STATUS1:
+ case MSG_EXTENDED_STATUS2:
+ case MSG_RETRY_DEFERRED:
+ break; // just here to prevent a warning
+ }
+ return true;
+}
+
+
+#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
+static struct mavlink_queue {
+ enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
+ uint8_t next_deferred_message;
+ uint8_t num_deferred_messages;
+} mavlink_queue[2];
+
+// send a message using mavlink
+static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
+{
+ uint8_t i, nextid;
+ struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
+
+ // see if we can send the deferred messages, if any
+ while (q->num_deferred_messages != 0) {
+ if (!mavlink_try_send_message(chan,
+ q->deferred_messages[q->next_deferred_message],
+ packet_drops)) {
+ break;
+ }
+ q->next_deferred_message++;
+ if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
+ q->next_deferred_message = 0;
+ }
+ q->num_deferred_messages--;
+ }
+
+ if (id == MSG_RETRY_DEFERRED) {
+ return;
+ }
+
+ // this message id might already be deferred
+ for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
+ if (q->deferred_messages[nextid] == id) {
+ // its already deferred, discard
+ return;
+ }
+ nextid++;
+ if (nextid == MAX_DEFERRED_MESSAGES) {
+ nextid = 0;
+ }
+ }
+
+ if (q->num_deferred_messages != 0 ||
+ !mavlink_try_send_message(chan, id, packet_drops)) {
+ // can't send it now, so defer it
+ if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
+ // the defer buffer is full, discard
+ return;
+ }
+ nextid = q->next_deferred_message + q->num_deferred_messages;
+ if (nextid >= MAX_DEFERRED_MESSAGES) {
+ nextid -= MAX_DEFERRED_MESSAGES;
+ }
+ q->deferred_messages[nextid] = id;
+ q->num_deferred_messages++;
+ }
+}
+
+void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
+{
+ if (severity == SEVERITY_LOW) {
+ // send via the deferred queuing system
+ mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status);
+ s->severity = (uint8_t)severity;
+ strncpy((char *)s->text, str, sizeof(s->text));
+ mavlink_send_message(chan, MSG_STATUSTEXT, 0);
+ } else {
+ // send immediately
+ mavlink_msg_statustext_send(chan, severity, str);
+ }
+}
+
+/*
+ default stream rates to 1Hz
+ */
+const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
+ // @Param: RAW_SENS
+ // @DisplayName: Raw sensor stream rate
+ // @Description: Raw sensor stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0], 1),
+
+ // @Param: EXT_STAT
+ // @DisplayName: Extended status stream rate to ground station
+ // @Description: Extended status stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1], 1),
+
+ // @Param: RC_CHAN
+ // @DisplayName: RC Channel stream rate to ground station
+ // @Description: RC Channel stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[2], 1),
+
+ // @Param: RAW_CTRL
+ // @DisplayName: Raw Control stream rate to ground station
+ // @Description: Raw Control stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3], 1),
+
+ // @Param: POSITION
+ // @DisplayName: Position stream rate to ground station
+ // @Description: Position stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 1),
+
+ // @Param: EXTRA1
+ // @DisplayName: Extra data type 1 stream rate to ground station
+ // @Description: Extra data type 1 stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 1),
+
+ // @Param: EXTRA2
+ // @DisplayName: Extra data type 2 stream rate to ground station
+ // @Description: Extra data type 2 stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 1),
+
+ // @Param: EXTRA3
+ // @DisplayName: Extra data type 3 stream rate to ground station
+ // @Description: Extra data type 3 stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 1),
+
+ // @Param: PARAMS
+ // @DisplayName: Parameter stream rate to ground station
+ // @Description: Parameter stream rate to ground station
+ // @Units: Hz
+ // @Range: 0 10
+ // @Increment: 1
+ // @User: Advanced
+ AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10),
+ AP_GROUPEND
+};
+
+
+GCS_MAVLINK::GCS_MAVLINK() :
+ packet_drops(0)
+{
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+void
+GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
+{
+ GCS_Class::init(port);
+ if (port == (AP_HAL::BetterStream*)hal.uartA) {
+ mavlink_comm_0_port = port;
+ chan = MAVLINK_COMM_0;
+ }else{
+ mavlink_comm_1_port = port;
+ chan = MAVLINK_COMM_1;
+ }
+ _queued_parameter = NULL;
+}
+
+void
+GCS_MAVLINK::update(void)
+{
+ // receive new packets
+ mavlink_message_t msg;
+ mavlink_status_t status;
+ status.packet_rx_drop_count = 0;
+
+ // process received bytes
+ uint16_t nbytes = comm_get_available(chan);
+ for (uint16_t i=0; i= NUM_STREAMS) {
+ return false;
+ }
+ float rate = (uint8_t)streamRates[stream_num].get();
+
+ // send at a much lower rate during parameter sends
+ if (_queued_parameter != NULL) {
+ rate *= 0.25;
+ }
+
+ if (rate <= 0) {
+ return false;
+ }
+
+ if (stream_ticks[stream_num] == 0) {
+ // we're triggering now, setup the next trigger point
+ if (rate > 50) {
+ rate = 50;
+ }
+ stream_ticks[stream_num] = (50 / rate) + stream_slowdown;
+ return true;
+ }
+
+ // count down at 50Hz
+ stream_ticks[stream_num]--;
+ return false;
+}
+
+void
+GCS_MAVLINK::data_stream_send(void)
+{
+ if (_queued_parameter != NULL) {
+ if (streamRates[STREAM_PARAMS].get() <= 0) {
+ streamRates[STREAM_PARAMS].set(10);
+ }
+ if (stream_trigger(STREAM_PARAMS)) {
+ send_message(MSG_NEXT_PARAM);
+ }
+ }
+
+ if (in_mavlink_delay) {
+ // don't send any other stream types while in the delay callback
+ return;
+ }
+
+ if (stream_trigger(STREAM_RAW_SENSORS)) {
+ send_message(MSG_RAW_IMU1);
+ send_message(MSG_RAW_IMU2);
+ send_message(MSG_RAW_IMU3);
+ }
+
+ if (stream_trigger(STREAM_EXTENDED_STATUS)) {
+ send_message(MSG_EXTENDED_STATUS1);
+ send_message(MSG_EXTENDED_STATUS2);
+ send_message(MSG_NAV_CONTROLLER_OUTPUT);
+ send_message(MSG_GPS_RAW);
+ }
+
+ if (stream_trigger(STREAM_POSITION)) {
+ send_message(MSG_LOCATION);
+ }
+
+ if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+ send_message(MSG_SERVO_OUT);
+ }
+
+ if (stream_trigger(STREAM_RC_CHANNELS)) {
+ send_message(MSG_RADIO_OUT);
+ }
+
+ if (stream_trigger(STREAM_EXTRA1)) {
+ send_message(MSG_ATTITUDE);
+ }
+
+ if (stream_trigger(STREAM_EXTRA3)) {
+ send_message(MSG_AHRS);
+ send_message(MSG_HWSTATUS);
+ }
+}
+
+
+
+void
+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; imsgid) {
+
+ 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; icopy_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);
+ 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
+ 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;
+ break;
+ }
+
+ 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;
+ }
+
+ default:
+ break;
+
+ } // end switch
+} // end handle mavlink
+
+uint16_t
+GCS_MAVLINK::_count_parameters()
+{
+ // if we haven't cached the parameter count yet...
+ if (0 == _parameter_count) {
+ AP_Param *vp;
+ AP_Param::ParamToken token;
+
+ vp = AP_Param::first(&token, NULL);
+ do {
+ _parameter_count++;
+ } while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
+ }
+ return _parameter_count;
+}
+
+/**
+ * @brief Send the next pending parameter, called from deferred message
+ * handling code
+ */
+void
+GCS_MAVLINK::queued_param_send()
+{
+ if (_queued_parameter == NULL) {
+ return;
+ }
+
+ uint16_t bytes_allowed;
+ uint8_t count;
+ uint32_t tnow = hal.scheduler->millis();
+
+ // use at most 30% of bandwidth on parameters. The constant 26 is
+ // 1/(1000 * 1/8 * 0.001 * 0.3)
+ bytes_allowed = g.serial3_baud * (tnow - _queued_parameter_send_time_ms) * 26;
+ if (bytes_allowed > comm_get_txspace(chan)) {
+ bytes_allowed = comm_get_txspace(chan);
+ }
+ count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+
+ while (_queued_parameter != NULL && count--) {
+ AP_Param *vp;
+ float value;
+
+ // copy the current parameter and prepare to move to the next
+ vp = _queued_parameter;
+
+ // if the parameter can be cast to float, report it here and break out of the loop
+ value = vp->cast_to_float(_queued_parameter_type);
+
+ char param_name[AP_MAX_NAME_SIZE];
+ vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
+
+ mavlink_msg_param_value_send(
+ chan,
+ param_name,
+ value,
+ mav_var_type(_queued_parameter_type),
+ _queued_parameter_count,
+ _queued_parameter_index);
+
+ _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
+ _queued_parameter_index++;
+ }
+ _queued_parameter_send_time_ms = tnow;
+}
+
+/*
+ * a delay() callback that processes MAVLink packets. We set this as the
+ * callback in long running library initialisation routines to allow
+ * MAVLink to process packets while waiting for the initialisation to
+ * complete
+ */
+static void mavlink_delay_cb()
+{
+ static uint32_t last_1hz, last_50hz, last_5s;
+ if (!gcs0.initialised) return;
+
+ in_mavlink_delay = true;
+
+ uint32_t tnow = hal.scheduler->millis();
+ if (tnow - last_1hz > 1000) {
+ last_1hz = tnow;
+ gcs_send_message(MSG_HEARTBEAT);
+ gcs_send_message(MSG_EXTENDED_STATUS1);
+ }
+ if (tnow - last_50hz > 20) {
+ last_50hz = tnow;
+ gcs_update();
+ gcs_data_stream_send();
+ notify.update();
+ }
+ if (tnow - last_5s > 5000) {
+ last_5s = tnow;
+ gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
+ }
+ in_mavlink_delay = false;
+}
+
+/*
+ * send a message on both GCS links
+ */
+static void gcs_send_message(enum ap_message id)
+{
+ gcs0.send_message(id);
+ if (gcs3.initialised) {
+ gcs3.send_message(id);
+ }
+}
+
+/*
+ * send data streams in the given rate range on both links
+ */
+static void gcs_data_stream_send(void)
+{
+ gcs0.data_stream_send();
+ if (gcs3.initialised) {
+ gcs3.data_stream_send();
+ }
+}
+
+/*
+ * look for incoming commands on the GCS links
+ */
+static void gcs_update(void)
+{
+ gcs0.update();
+ if (gcs3.initialised) {
+ gcs3.update();
+ }
+}
+
+static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
+{
+ gcs0.send_text_P(severity, str);
+ if (gcs3.initialised) {
+ gcs3.send_text_P(severity, str);
+ }
+#if LOGGING_ENABLED == ENABLED
+ DataFlash.Log_Write_Message_P(str);
+#endif
+}
+
+/*
+ * send a low priority formatted message to the GCS
+ * only one fits in the queue, so if you send more than one before the
+ * last one gets into the serial buffer then the old one will be lost
+ */
+void gcs_send_text_fmt(const prog_char_t *fmt, ...)
+{
+ va_list arg_list;
+ gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW;
+ va_start(arg_list, fmt);
+ hal.util->vsnprintf_P((char *)gcs0.pending_status.text,
+ sizeof(gcs0.pending_status.text), fmt, arg_list);
+ va_end(arg_list);
+#if LOGGING_ENABLED == ENABLED
+ DataFlash.Log_Write_Message(gcs0.pending_status.text);
+#endif
+ gcs3.pending_status = gcs0.pending_status;
+ mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
+ if (gcs3.initialised) {
+ mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
+ }
+}
+
diff --git a/Tools/AntennaTracker/Makefile b/Tools/AntennaTracker/Makefile
new file mode 100644
index 0000000000..3959a1654b
--- /dev/null
+++ b/Tools/AntennaTracker/Makefile
@@ -0,0 +1 @@
+include ../../mk/apm.mk
diff --git a/Tools/AntennaTracker/Parameters.h b/Tools/AntennaTracker/Parameters.h
new file mode 100644
index 0000000000..89db93f5a6
--- /dev/null
+++ b/Tools/AntennaTracker/Parameters.h
@@ -0,0 +1,101 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef PARAMETERS_H
+#define PARAMETERS_H
+
+#include
+
+// Global parameter class.
+//
+class Parameters {
+public:
+
+ /*
+ * The value of k_format_version determines whether the existing
+ * eeprom data is considered valid. You should only change this
+ * value under the following circumstances:
+ *
+ * 1) the meaning of an existing eeprom parameter changes
+ *
+ * 2) the value of an existing k_param_* enum value changes
+ *
+ * Adding a new parameter should _not_ require a change to
+ * k_format_version except under special circumstances. If you
+ * change it anyway then all ArduPlane users will need to reload all
+ * their parameters. We want that to be an extremely rare
+ * thing. Please do not just change it "just in case".
+ *
+ * To determine if a k_param_* value has changed, use the rules of
+ * C++ enums to work out the value of the neighboring enum
+ * values. If you don't know the C++ enum rules then please ask for
+ * help.
+ */
+
+ //////////////////////////////////////////////////////////////////
+ // STOP!!! DO NOT CHANGE THIS VALUE UNTIL YOU FULLY UNDERSTAND THE
+ // COMMENTS ABOVE. IF UNSURE, ASK ANOTHER DEVELOPER!!!
+ static const uint16_t k_format_version = 1;
+ //////////////////////////////////////////////////////////////////
+
+
+ // The parameter software_type is set up solely for ground station use
+ // and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
+ // GCS will interpret values 0-9 as ArduPilotMega. Developers may use
+ // values within that range to identify different branches.
+ //
+ static const uint16_t k_software_type = 4;
+
+ enum {
+ // Layout version number, always key zero.
+ //
+ k_param_format_version = 0,
+ k_param_software_type,
+
+ k_param_gcs0 = 100, // stream rates for port0
+ k_param_gcs3, // stream rates for port3
+ k_param_sysid_this_mav,
+ k_param_sysid_my_gcs,
+ k_param_serial0_baud,
+ k_param_serial3_baud,
+ k_param_imu,
+ k_param_compass_enabled,
+ k_param_compass,
+ k_param_ahrs, // AHRS group
+ k_param_barometer,
+ k_param_scheduler,
+ k_param_ins,
+ k_param_sitl,
+ k_param_pidPitch2Srv,
+ k_param_pidYaw2Srv,
+
+ k_param_channel_yaw = 200,
+ k_param_channel_pitch
+
+ // 254,255: reserved
+ };
+
+ AP_Int16 format_version;
+ AP_Int8 software_type;
+
+ // Telemetry control
+ //
+ AP_Int16 sysid_this_mav;
+ AP_Int16 sysid_my_gcs;
+ AP_Int8 serial0_baud;
+ AP_Int8 serial3_baud;
+
+ AP_Int8 compass_enabled;
+
+ // PID controllers
+ PID pidPitch2Srv;
+ PID pidYaw2Srv;
+
+ Parameters() :
+ pidPitch2Srv(1.0f, 0.2f, 0.05f, 4000.0f),
+ pidYaw2Srv(1.0f, 0.2f, 0.05f, 4000.0f)
+ {}
+};
+
+extern const AP_Param::Info var_info[];
+
+#endif // PARAMETERS_H
diff --git a/Tools/AntennaTracker/Parameters.pde b/Tools/AntennaTracker/Parameters.pde
new file mode 100644
index 0000000000..655f247226
--- /dev/null
+++ b/Tools/AntennaTracker/Parameters.pde
@@ -0,0 +1,125 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ * AntennaTracker parameter definitions
+ *
+ */
+
+#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
+#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} }
+#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
+
+const AP_Param::Info var_info[] PROGMEM = {
+ GSCALAR(format_version, "FORMAT_VERSION", 0),
+ GSCALAR(software_type, "SYSID_SW_TYPE", Parameters::k_software_type),
+
+ // @Param: SYSID_THISMAV
+ // @DisplayName: MAVLink system ID
+ // @Description: The identifier of this device in the MAVLink protocol
+ // @Range: 1 255
+ // @User: Advanced
+ GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID),
+
+ // @Param: SYSID_MYGCS
+ // @DisplayName: Ground station MAVLink system ID
+ // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match.
+ // @Range: 1 255
+ // @User: Advanced
+ GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
+
+ // @Param: SERIAL0_BAUD
+ // @DisplayName: USB Console Baud Rate
+ // @Description: The baud rate used on the main uart
+ // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
+ // @User: Standard
+ GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000),
+
+ // @Param: SERIAL3_BAUD
+ // @DisplayName: Telemetry Baud Rate
+ // @Description: The baud rate used on the telemetry port
+ // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200
+ // @User: Standard
+ GSCALAR(serial3_baud, "SERIAL3_BAUD", SERIAL3_BAUD/1000),
+
+
+
+ // @Param: MAG_ENABLE
+ // @DisplayName: Enable Compass
+ // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1.
+ // @Values: 0:Disabled,1:Enabled
+ // @User: Standard
+ GSCALAR(compass_enabled, "MAG_ENABLE", 1),
+
+ // barometer ground calibration. The GND_ prefix is chosen for
+ // compatibility with previous releases of ArduPlane
+ // @Group: GND_
+ // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+ GOBJECT(barometer, "GND_", AP_Baro),
+
+ // @Group: COMPASS_
+ // @Path: ../libraries/AP_Compass/Compass.cpp
+ GOBJECT(compass, "COMPASS_", Compass),
+
+ // @Group: SCHED_
+ // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
+ GOBJECT(scheduler, "SCHED_", AP_Scheduler),
+
+ // @Group: SR0_
+ // @Path: GCS_Mavlink.pde
+ GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
+
+ // @Group: SR3_
+ // @Path: GCS_Mavlink.pde
+ GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
+
+ // @Group: INS_
+ // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
+ GOBJECT(ins, "INS_", AP_InertialSensor),
+
+ // @Group: AHRS_
+ // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
+ GOBJECT(ahrs, "AHRS_", AP_AHRS),
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+ // @Group: SIM_
+ // @Path: ../libraries/SITL/SITL.cpp
+ GOBJECT(sitl, "SIM_", SITL),
+#endif
+
+ // RC channel
+ //-----------
+ // @Group: RC1_
+ // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+ GOBJECT(channel_yaw, "RC1_", RC_Channel),
+
+ // @Group: RC2_
+ // @Path: ../libraries/RC_Channel/RC_Channel.cpp
+ GOBJECT(channel_pitch, "RC2_", RC_Channel),
+
+ GGROUP(pidPitch2Srv, "PITCH2SRV_", PID),
+ GGROUP(pidYaw2Srv, "YAW2SRV_", PID),
+
+ AP_VAREND
+};
+
+
+static void load_parameters(void)
+{
+ if (!g.format_version.load() ||
+ g.format_version != Parameters::k_format_version) {
+
+ // erase all parameters
+ cliSerial->printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
+ AP_Param::erase_all();
+
+ // save the current format version
+ g.format_version.set_and_save(Parameters::k_format_version);
+ cliSerial->println_P(PSTR("done."));
+ } else {
+ uint32_t before = hal.scheduler->micros();
+ // Load all auto-loaded EEPROM variables
+ AP_Param::load_all();
+ cliSerial->printf_P(PSTR("load_all took %luus\n"), hal.scheduler->micros() - before);
+ }
+}
diff --git a/Tools/AntennaTracker/config.h b/Tools/AntennaTracker/config.h
new file mode 100644
index 0000000000..c34ccdaab7
--- /dev/null
+++ b/Tools/AntennaTracker/config.h
@@ -0,0 +1,83 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+#include "defines.h"
+
+#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
+
+///
+/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
+/// change in your local copy of APM_Config.h.
+///
+
+// Just so that it's completely clear...
+#define ENABLED 1
+#define DISABLED 0
+
+// this avoids a very common config error
+#define ENABLE ENABLED
+#define DISABLE DISABLED
+
+//////////////////////////////////////////////////////////////////////////////
+// main board differences
+//
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+ # define CONFIG_INS_TYPE CONFIG_INS_OILPAN
+ # define CONFIG_BARO AP_BARO_BMP085
+ # define CONFIG_COMPASS AP_COMPASS_HMC5843
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ # define CONFIG_INS_TYPE CONFIG_INS_MPU6000
+ # define CONFIG_BARO AP_BARO_MS5611
+ # define CONFIG_MS5611_SERIAL AP_BARO_MS5611_SPI
+ # define CONFIG_COMPASS AP_COMPASS_HMC5843
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+ # define CONFIG_INS_TYPE CONFIG_INS_HIL
+ # define CONFIG_BARO AP_BARO_HIL
+ # define CONFIG_COMPASS AP_COMPASS_HIL
+#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
+ # define CONFIG_INS_TYPE CONFIG_INS_PX4
+ # define CONFIG_BARO AP_BARO_PX4
+ # define CONFIG_COMPASS AP_COMPASS_PX4
+ # define SERIAL0_BAUD 115200
+#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
+ # define CONFIG_INS_TYPE CONFIG_INS_FLYMAPLE
+ # define CONFIG_BARO AP_BARO_BMP085
+ # define CONFIG_COMPASS AP_COMPASS_HMC5843
+ # define SERIAL0_BAUD 115200
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+ # define CONFIG_INS_TYPE CONFIG_INS_L3G4200D
+ # define CONFIG_BARO AP_BARO_BMP085
+ # define CONFIG_COMPASS AP_COMPASS_HMC5843
+#endif
+
+
+#ifndef CONFIG_BARO
+ # error "CONFIG_BARO not set"
+#endif
+
+#ifndef CONFIG_COMPASS
+ # error "CONFIG_COMPASS not set"
+#endif
+
+#ifndef MAV_SYSTEM_ID
+ // use 2 for antenna tracker by default
+ # define MAV_SYSTEM_ID 2
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Serial port speeds.
+//
+#ifndef SERIAL0_BAUD
+ # define SERIAL0_BAUD 115200
+#endif
+#ifndef SERIAL3_BAUD
+ # define SERIAL3_BAUD 57600
+#endif
+
+
+#ifndef SERIAL_BUFSIZE
+ # define SERIAL_BUFSIZE 512
+#endif
+
+#ifndef SERIAL2_BUFSIZE
+ # define SERIAL2_BUFSIZE 256
+#endif
diff --git a/Tools/AntennaTracker/defines.h b/Tools/AntennaTracker/defines.h
new file mode 100644
index 0000000000..85da6ec478
--- /dev/null
+++ b/Tools/AntennaTracker/defines.h
@@ -0,0 +1,56 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef _DEFINES_H
+#define _DEFINES_H
+
+#define ToRad(x) radians(x) // *pi/180
+#define ToDeg(x) degrees(x) // *180/pi
+
+/// please keep each MSG_ to a single MAVLink message. If need be
+/// create new MSG_ IDs for additional messages on the same
+/// stream
+enum ap_message {
+ MSG_HEARTBEAT,
+ MSG_ATTITUDE,
+ MSG_LOCATION,
+ MSG_AHRS,
+ MSG_HWSTATUS,
+ MSG_GPS_RAW,
+ MSG_SERVO_OUT,
+ MSG_RADIO_OUT,
+ MSG_RAW_IMU1,
+ MSG_RAW_IMU2,
+ MSG_RAW_IMU3,
+ MSG_NEXT_PARAM,
+ MSG_STATUSTEXT,
+ MSG_EXTENDED_STATUS1,
+ MSG_EXTENDED_STATUS2,
+ MSG_NAV_CONTROLLER_OUTPUT,
+ MSG_RETRY_DEFERRED // this must be last
+};
+
+#define EEPROM_MAX_ADDR 4096
+
+// mark a function as not to be inlined
+#define NOINLINE __attribute__((noinline))
+
+// InertialSensor driver types
+#define CONFIG_INS_OILPAN 1
+#define CONFIG_INS_MPU6000 2
+#define CONFIG_INS_HIL 3
+#define CONFIG_INS_PX4 4
+#define CONFIG_INS_FLYMAPLE 5
+#define CONFIG_INS_L3G4200D 6
+
+// barometer driver types
+#define AP_BARO_BMP085 1
+#define AP_BARO_MS5611 2
+#define AP_BARO_PX4 3
+#define AP_BARO_HIL 4
+
+// compass driver types
+#define AP_COMPASS_HMC5843 1
+#define AP_COMPASS_PX4 2
+#define AP_COMPASS_HIL 3
+
+#endif // _DEFINES_H
diff --git a/Tools/AntennaTracker/sensors.pde b/Tools/AntennaTracker/sensors.pde
new file mode 100644
index 0000000000..ee9b351c4f
--- /dev/null
+++ b/Tools/AntennaTracker/sensors.pde
@@ -0,0 +1,65 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static void init_barometer(void)
+{
+ gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
+ barometer.calibrate();
+ gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
+}
+
+// read the barometer and return the updated altitude in meters
+static void update_barometer(void)
+{
+ barometer.read();
+}
+
+
+/*
+ update INS and attitude
+ */
+static void update_ahrs()
+{
+ ahrs.update();
+}
+
+
+/*
+ read and update compass
+ */
+static void update_compass(void)
+{
+ if (g.compass_enabled && compass.read()) {
+ ahrs.set_compass(&compass);
+ compass.null_offsets();
+ } else {
+ ahrs.set_compass(NULL);
+ }
+}
+
+/*
+ if the compass is enabled then try to accumulate a reading
+ */
+static void compass_accumulate(void)
+{
+ if (g.compass_enabled) {
+ compass.accumulate();
+ }
+}
+
+/*
+ try to accumulate a baro reading
+ */
+static void barometer_accumulate(void)
+{
+ barometer.accumulate();
+}
+
+
+/*
+ read the GPS
+ */
+static void update_GPS(void)
+{
+ g_gps->update();
+}
+
diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde
new file mode 100644
index 0000000000..4e6aad0860
--- /dev/null
+++ b/Tools/AntennaTracker/system.pde
@@ -0,0 +1,109 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static void init_tracker()
+{
+ hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE);
+
+ // gps port
+ hal.uartB->begin(38400, 256, 16);
+
+ cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
+ "\n\nFree RAM: %u\n"),
+ memcheck_available_memory());
+
+ // Check the EEPROM format version before loading any parameters from EEPROM
+ load_parameters();
+
+ // reset the uartA baud rate after parameter load
+ hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD));
+
+ // init baro before we start the GCS, so that the CLI baro test works
+ barometer.init();
+
+ // init the GCS
+ gcs0.init(hal.uartA);
+ // 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);
+
+ // we have a 2nd serial port for telemetry
+ hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
+ 128, SERIAL2_BUFSIZE);
+ gcs3.init(hal.uartC);
+
+ mavlink_system.sysid = g.sysid_this_mav;
+
+ if (g.compass_enabled==true) {
+ if (!compass.init() || !compass.read()) {
+ cliSerial->println_P(PSTR("Compass initialisation failed!"));
+ g.compass_enabled = false;
+ } else {
+ ahrs.set_compass(&compass);
+ }
+ }
+
+ // Do GPS init
+ g_gps = &g_gps_driver;
+
+ // GPS Initialization
+ g_gps->init(hal.uartB, GPS::GPS_ENGINE_STATIONARY);
+
+ mavlink_system.compid = 4;
+ mavlink_system.type = MAV_TYPE_ANTENNA_TRACKER;
+
+ ahrs.init();
+ ahrs.set_fly_forward(false);
+
+ ins.init(AP_InertialSensor::WARM_START, ins_sample_rate);
+ ahrs.reset();
+
+ init_barometer();
+
+ hal.uartA->set_blocking_writes(false);
+ hal.uartC->set_blocking_writes(false);
+
+ // setup antenna control PWM channels
+ channel_yaw.set_angle(4500);
+ channel_pitch.set_angle(4500);
+
+ channel_yaw.output_trim();
+ channel_pitch.output_trim();
+
+ channel_yaw.calc_pwm();
+ channel_pitch.calc_pwm();
+
+ channel_yaw.enable_out();
+ channel_pitch.enable_out();
+
+ gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
+ hal.scheduler->delay(1000);
+}
+
+// updates the status of the notify objects
+// should be called at 50hz
+static void update_notify()
+{
+ notify.update();
+}
+
+/*
+ * map from a 8 bit EEPROM baud rate to a real baud rate
+ */
+static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
+{
+ switch (rate) {
+ case 1: return 1200;
+ case 2: return 2400;
+ case 4: return 4800;
+ case 9: return 9600;
+ case 19: return 19200;
+ case 38: return 38400;
+ case 57: return 57600;
+ case 111: return 111100;
+ case 115: return 115200;
+ }
+ cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD"));
+ return default_baud;
+}
+
+
diff --git a/Tools/AntennaTracker/tracking.pde b/Tools/AntennaTracker/tracking.pde
new file mode 100644
index 0000000000..b8f0d7b6b1
--- /dev/null
+++ b/Tools/AntennaTracker/tracking.pde
@@ -0,0 +1,81 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/**
+ state of the vehicle we are tracking
+ */
+static struct {
+ Location location;
+ uint32_t last_update_us;
+ float heading; // degrees
+ float ground_speed; // m/s
+} vehicle;
+
+static Location our_location;
+
+/**
+ update the pitch servo. The aim is to drive the boards pitch to the
+ requested pitch
+ */
+static void update_pitch_servo(float pitch)
+{
+ channel_pitch.servo_out = g.pidPitch2Srv.get_pid_4500(degrees(ahrs.pitch) - pitch);
+ channel_pitch.calc_pwm();
+ channel_pitch.output();
+}
+
+/**
+ update the yaw servo
+ */
+static void update_yaw_servo(float yaw)
+{
+ channel_yaw.servo_out = g.pidYaw2Srv.get_pid_4500(degrees(ahrs.yaw) - yaw);
+ channel_yaw.calc_pwm();
+ channel_yaw.output();
+}
+
+
+/**
+ main antenna tracking code, called at 50Hz
+ */
+static void update_tracking(void)
+{
+ // project the vehicle position to take account of lost radio packets
+ Location vpos = vehicle.location;
+ float dt = (hal.scheduler->micros() - vehicle.last_update_us) * 1.0e-6f;
+ location_update(vpos, vehicle.heading, vehicle.ground_speed * dt);
+
+ // update our position if we have at least a 2D fix
+ if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
+ our_location.lat = g_gps->latitude;
+ our_location.lng = g_gps->longitude;
+ our_location.alt = 0; // assume ground level for now
+ }
+
+ // calculate the bearing to the vehicle
+ float bearing = get_bearing_cd(our_location, vehicle.location) * 0.01f;
+ float distance = get_distance(our_location, vehicle.location);
+ float pitch = degrees(atan2(vehicle.location.alt - our_location.alt, distance));
+
+ // update the servos
+ update_pitch_servo(pitch);
+ update_yaw_servo(bearing);
+
+ // update nav_status for NAV_CONTROLLER_OUTPUT
+ nav_status.bearing = bearing;
+ nav_status.pitch = pitch;
+ nav_status.distance = distance;
+}
+
+
+/**
+ handle an updated position from the aircraft
+ */
+static void tracking_update_position(const mavlink_global_position_int_t &msg)
+{
+ vehicle.location.lat = msg.lat;
+ vehicle.location.lng = msg.lon;
+ vehicle.location.alt = msg.relative_alt/10;
+ vehicle.heading = msg.hdg * 0.01f;
+ vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f;
+ vehicle.last_update_us = hal.scheduler->micros();
+}