aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AntennaTracker: update to build with latest libraries

Browse Source 
AntennaTracker has not been maintained as other parts of the system and
libraries have been modernised. This patch at least gets it to the stage where
it compiles and runs in SITL. Also added Tools/autotest/sim_antennatracker.sh
to run in SITL
This commit is contained in:
Mike McCauley 2014-02-18 17:55:32 +10:00 committed by Andrew Tridgell
parent 9a869fb7e5
commit 8f71af605b
6 changed files with 127 additions and 309 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -50,6 +50,7 @@
#include <SITL.h>
#include <PID.h>
#include <AP_Scheduler.h> // main loop scheduler
#include <AP_NavEKF.h>
#include <AP_Vehicle.h>
#include <AP_Notify.h> // 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

187
Tools/AntennaTracker/GCS.h
View File

@ -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 <AP_HAL.h>
#include <AP_Common.h>
#include <GPS.h>
#include <stdint.h>
///
/// @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

138
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -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);
}

23
Tools/AntennaTracker/defines.h
View File

@ -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

2
Tools/AntennaTracker/system.pde
View File

@ -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();

71
Tools/autotest/sim_antennatracker.sh Executable file
View File

@ -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 $*