diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde index f1b67fc1a5..d468672e09 100644 --- a/Tools/AntennaTracker/AntennaTracker.pde +++ b/Tools/AntennaTracker/AntennaTracker.pde @@ -50,6 +50,7 @@ #include #include #include // main loop scheduler +#include #include #include // Notify library @@ -162,7 +163,12 @@ AP_InertialSensor_L3G4200D ins; #error Unrecognised CONFIG_INS_TYPE setting. #endif // CONFIG_INS_TYPE -AP_AHRS_DCM ahrs(&ins, g_gps); +// Inertial Navigation EKF +#if AP_AHRS_NAVEKF_AVAILABLE +AP_AHRS_NavEKF ahrs(ins, barometer, g_gps); +#else +AP_AHRS_DCM ahrs(ins, barometer, g_gps); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL SITL sitl; @@ -198,6 +204,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { compass_accumulate, 1, 1500 }, { barometer_accumulate, 1, 900 }, { update_notify, 1, 100 }, + { gcs_retry_deferred, 1, 1000 }, { one_second_loop, 50, 3900 } }; @@ -209,10 +216,6 @@ AP_Param param_loader(var_info, EEPROM_MAX_ADDR); */ 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[] @@ -223,7 +226,7 @@ void setup() AP_Notify::flags.pre_arm_check = true; AP_Notify::flags.failsafe_battery = false; - notify.init(); + notify.init(false); init_tracker(); // initialise the main loop scheduler diff --git a/Tools/AntennaTracker/GCS.h b/Tools/AntennaTracker/GCS.h deleted file mode 100644 index d0f34c737f..0000000000 --- a/Tools/AntennaTracker/GCS.h +++ /dev/null @@ -1,187 +0,0 @@ -// -*- 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 index 9d16055c11..7869783a25 100644 --- a/Tools/AntennaTracker/GCS_Mavlink.pde +++ b/Tools/AntennaTracker/GCS_Mavlink.pde @@ -107,8 +107,9 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan) static void NOINLINE send_raw_imu1(mavlink_channel_t chan) { - Vector3f accel = ins.get_accel(); - Vector3f gyro = ins.get_gyro(); + const Vector3f &accel = ins.get_accel(); + const Vector3f &gyro = ins.get_gyro(); + const Vector3f &mag = compass.get_field(); mavlink_msg_raw_imu_send( chan, @@ -119,9 +120,31 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan) gyro.x * 1000.0, gyro.y * 1000.0, gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); + mag.x, + mag.y, + mag.z); + + if (ins.get_gyro_count() <= 1 && + ins.get_accel_count() <= 1 && + compass.get_count() <= 1) { + return; + } + const Vector3f &accel2 = ins.get_accel(1); + const Vector3f &gyro2 = ins.get_gyro(1); + const Vector3f &mag2 = compass.get_field(1); + mavlink_msg_scaled_imu2_send( + chan, + hal.scheduler->millis(), + accel2.x * 1000.0f / GRAVITY_MSS, + accel2.y * 1000.0f / GRAVITY_MSS, + accel2.z * 1000.0f / GRAVITY_MSS, + gyro2.x * 1000.0f, + gyro2.y * 1000.0f, + gyro2.z * 1000.0f, + mag2.x, + mag2.y, + mag2.z); + } static void NOINLINE send_raw_imu2(mavlink_channel_t chan) @@ -214,7 +237,6 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) 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); @@ -289,6 +311,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, case MSG_EXTENDED_STATUS1: case MSG_EXTENDED_STATUS2: case MSG_RETRY_DEFERRED: + case MSG_CURRENT_WAYPOINT: + case MSG_VFR_HUD: + case MSG_RADIO_IN: + case MSG_SYSTEM_TIME: + case MSG_NEXT_WAYPOINT: + case MSG_LIMITS_STATUS: + case MSG_FENCE_STATUS: + case MSG_SIMSTATE: + case MSG_WIND: + case MSG_RANGEFINDER: break; // just here to prevent a warning } return true; @@ -455,27 +487,6 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { 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) { @@ -814,71 +825,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } // 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 @@ -979,3 +925,11 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...) } } +/** + retry any deferred messages + */ +static void gcs_retry_deferred(void) +{ + gcs_send_message(MSG_RETRY_DEFERRED); +} + diff --git a/Tools/AntennaTracker/defines.h b/Tools/AntennaTracker/defines.h index 85da6ec478..9ac9bca658 100644 --- a/Tools/AntennaTracker/defines.h +++ b/Tools/AntennaTracker/defines.h @@ -6,29 +6,6 @@ #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 diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde index 4e6aad0860..0a25ca1890 100644 --- a/Tools/AntennaTracker/system.pde +++ b/Tools/AntennaTracker/system.pde @@ -9,7 +9,7 @@ static void init_tracker() cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %u\n"), - memcheck_available_memory()); + hal.util->available_memory()); // Check the EEPROM format version before loading any parameters from EEPROM load_parameters(); diff --git a/Tools/autotest/sim_antennatracker.sh b/Tools/autotest/sim_antennatracker.sh new file mode 100755 index 0000000000..ed6d916dd1 --- /dev/null +++ b/Tools/autotest/sim_antennatracker.sh @@ -0,0 +1,71 @@ +#!/bin/bash + +# home location lat, lon, alt, heading +SIMHOME="-35.363261,149.165230,584,353" + +# check the instance number to allow for multiple copies of the sim running at once +INSTANCE=0 + +# Try to run a command in an appropriate type of terminal window +# depending on whats available +# Sigh: theres no common way of handling command line args :-( +function run_in_terminal_window() +{ + if [ -x /usr/bin/konsole ]; then + /usr/bin/konsole --hold -e $* + elif [ -x /usr/bin/gnome-terminal ]; then + /usr/bin/gnome-terminal -e "$*" + elif [ -x /usr/bin/xterm ]; then + /usr/bin/xterm -hold -e $* & + else + # out of options: run in the background + $* & + fi +} + +# parse options. Thanks to http://wiki.bash-hackers.org/howto/getopts_tutorial +while getopts ":I:" opt; do + case $opt in + I) + INSTANCE=$OPTARG + ;; + \?) + # allow other args to pass on to mavproxy + break + ;; + :) + echo "Option -$OPTARG requires an argument." >&2 + exit 1 + esac +done +shift $((OPTIND-1)) + +# kill existing copy if this is the '0' instance only +[ "$INSTANCE" -eq "0" ] && { + killall -q AntennaTracker.elf +} + + +# setup ports for this instance +MAVLINK_PORT="tcp:127.0.0.1:"$((5760+10*$INSTANCE)) + +set -e +set -x + +autotest=$(dirname $(readlink -e $0)) +pushd $autotest/../../Tools/AntennaTracker +#make clean sitl +make sitl + +tfile=$(mktemp) +echo r > $tfile +#run_in_terminal_window gdb -x $tfile --args /tmp/AntennaTracker.build/AntennaTracker.elf +#run_in_terminal_window /tmp/AntennaTracker.build/AntennaTracker.elf -I$INSTANCE +run_in_terminal_window /tmp/AntennaTracker.build/AntennaTracker.elf -I$INSTANCE + +#run_in_terminal_window valgrind -q /tmp/AntennaTracker.build/AntennaTracker.elf +sleep 2 +rm -f $tfile +popd + +mavproxy.py --master $MAVLINK_PORT $*