Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: removed unused/unmaintained ArduTracker

Browse Source 
this has not compiled for a long time.
This commit is contained in:
Andrew Tridgell 2013-02-23 09:30:50 +11:00
parent e0506bd622
commit 74439b4f17
42 changed files with 0 additions and 11797 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Tools/ArduTracker/.ArduTracker.pde.swo
View File

Binary file not shown.

927
Tools/ArduTracker/APM_Config.h.reference
View File

@ -1,927 +0,0 @@
//
// Example and reference ArduPilot Mega configuration file
// =======================================================
//
// This file contains documentation and examples of configuration options
// supported by the ArduPilot Mega software.
//
// Most of these options are just that - optional. You should create
// the APM_Config.h file and use this file as a reference for options
// that you want to change. Don't copy this file directly; the options
// described here and their default values may change over time.
//
// Each item is marked with a keyword describing when you should set it:
//
// REQUIRED
// You must configure this in your APM_Config.h file. The
// software will not compile if the option is not set.
//
// OPTIONAL
// The option has a sensible default (which will be described
// here), but you may wish to override it.
//
// EXPERIMENTAL
// You should configure this option unless you are prepared
// to deal with potential problems. It may relate to a feature
// still in development, or which is not yet adequately tested.
//
// DEBUG
// The option should only be set if you are debugging the
// software, or if you are gathering information for a bug
// report.
//
// NOTE:
// Many of these settings are considered 'factory defaults', and the
// live value is stored and managed in the ArduPilot Mega EEPROM.
// Use the setup 'factoryreset' command after changing options in
// your APM_Config.h file.
//
// Units
// -----
//
// Unless indicated otherwise, numeric quantities use the following units:
//
// Measurement | Unit
// ------------+-------------------------------------
// angle | degrees
// distance | metres
// speed | metres per second
// servo angle | microseconds
// voltage | volts
// times | seconds
// throttle | percent
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL REQUIRED
//
// GPS configuration, must be one of:
//
// GPS_PROTOCOL_NONE No GPS attached
// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU.
// GPS_PROTOCOL_MTK MediaTek-based GPS for Mediatek 1.4
// GPS_PROTOCOL_MTK19 MediaTek-based GPS for firmware 1.6, 1.7, 1.8, 1.9
// GPS_PROTOCOL_UBLOX UBLOX GPS
// GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED
// GPS_PROTOCOL_NMEA Standard NMEA GPS. NOT SUPPORTED (yet?)
// GPS_PROTOCOL_AUTO 7
//
//#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX
//
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_SENSOR OPTIONAL
// AIRSPEED_RATIO OPTIONAL
//
// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached.
// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed
// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s
//
// The default assumes that an airspeed sensor is connected.
//
//#define AIRSPEED_SENSOR ENABLED
//#define AIRSPEED_RATIO 1.9936
//
//////////////////////////////////////////////////////////////////////////////
// HIL_PROTOCOL OPTIONAL
// HIL_MODE OPTIONAL
// HIL_PORT OPTIONAL
//
// Configuration for Hardware-in-the-loop simulation. In these modes,
// APM is connected via one or more interfaces to simulation software
// running on another system.
//
// HIL_PROTOCOL_XPLANE Configure for the X-plane HIL interface.
// HIL_PROTOCOL_MAVLINK Configure for HIL over MAVLink.
//
// HIL_MODE_DISABLED Configure for standard flight.
// HIL_MODE_ATTITUDE Simulator provides attitude and position information.
// HIL_MODE_SENSORS Simulator provides raw sensor values.
//
// Note that currently HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE.
// Note that currently HIL_PROTOCOL_MAVLINK requires HIL_MODE_SENSORS.
//
//#define HIL_MODE HIL_MODE_DISABLED
//#define HIL_PORT 0
//
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL OPTIONAL
// GCS_PORT OPTIONAL
//
// The GCS_PROTOCOL option determines which (if any) ground control station
// protocol will be used. Must be one of:
//
// GCS_PROTOCOL_NONE No GCS output
// GCS_PROTOCOL_STANDARD standard APM protocol
// GCS_PROTOCOL_LEGACY legacy ArduPilot protocol
// GCS_PROTOCOL_DEBUGTERMINAL In-flight debug console (experimental)
// GCS_PROTOCOL_MAVLINK QGroundControl protocol
//
// The GCS_PORT option determines which serial port will be used by the
// GCS protocol. The usual values are 0 for the console/USB port,
// or 3 for the telemetry port on the oilpan. Note that some protocols
// will ignore this value and always use the console port.
//
// The default serial port is the telemetry port for GCS_PROTOCOL_STANDARD
// and GCS_PROTOCOL_LEGACY. For all other protocols, the default serial
// port is the FTDI/console port. GCS_PORT normally should not be set
// in your configuration.
//
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define GCS_PORT 3
//
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
// SERIAL0_BAUD OPTIONAL
//
// Baudrate for the console port. Default is 38400bps.
//
// SERIAL3_BAUD OPTIONAL
//
// Baudrate for the telemetry port. Default is 115200bps.
//
//#define SERIAL0_BAUD 115200
//#define SERIAL3_BAUD 57600
//
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring OPTIONAL
//
// See the manual for details on selecting divider resistors for battery
// monitoring via the oilpan.
//
// BATTERY_EVENT OPTIONAL
//
// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is
// DISABLED.
//
// BATTERY_TYPE OPTIONAL if BATTERY_EVENT is set
//
// Set to 0 for 3s LiPo, 1 for 4s LiPo. The default is 0, selecting a 3s
// battery.
//
// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set.
//
// Normally derived from BATTERY_TYPE, the automatic value can be overridden
// here. Value in volts at which ArduPilot Mega should consider the
// battery to be "low".
//
// VOLT_DIV_RATIO OPTIONAL
//
// See the manual for details. The default value corresponds to the resistors
// recommended by the manual.
//
//#define BATTERY_EVENT DISABLED
//#define BATTERY_TYPE 0
//#define LOW_VOLTAGE 11.4
//#define VOLT_DIV_RATIO 3.0
//
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE OPTIONAL
//
// In order to have accurate pressure and battery voltage readings, this
// value should be set to the voltage measured on the 5V rail on the oilpan.
//
// See the manual for more details. The default value should be close.
//
//#define INPUT_VOLTAGE 5.0
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE OPTIONAL
// FLIGHT_MODE_CHANNEL OPTIONAL
//
// Flight modes assigned to the control channel, and the input channel that
// is read for the control mode.
//
// Use a servo tester, or the ArduPilotMega_demo test program to check your
// switch settings.
//
// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and
// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option
// uses channel numbers 1-8 (and defaults to 8).
//
// If you only have a three-position switch or just want three modes, set your
// switch to produce 1165, 1425, and 1815 microseconds and configure
// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default.
//
// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control
// channel connected to input channel 8, the hardware failsafe mode will
// activate for any control input over 1750ms.
//
// For more modes (up to six), set your switch(es) to produce any of 1165,
// 1295, 1425, 1555, 1685, and 1815 microseconds.
//
// Flight mode | Switch Setting (ms)
// ------------+---------------------
// 1 | 1165
// 2 | 1295
// 3 | 1425
// 4 | 1555
// 5 | 1685
// 6 | 1815 (FAILSAFE if using channel 8)
//
// The following standard flight modes are available:
//
// Name | Description
// -----------------+--------------------------------------------
// |
// MANUAL | Full manual control via the hardware multiplexer.
// |
// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs.
// |
// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle.
// |
// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle.
// |
// RTL | Returns to the Home location and then LOITERs at a safe altitude.
// |
// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter
// | application or your Ground Control System to edit and upload
// | waypoints and other commands.
// |
//
//
// The following non-standard modes are EXPERIMENTAL:
//
// Name | Description
// -----------------+--------------------------------------------
// |
// LOITER | Flies in a circle around the current location.
// |
// CIRCLE | Flies in a stabilized 'dumb' circle.
// |
//
//
// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and
// FLIGHT_MODE_6 should be MANUAL.
//
//
//#define FLIGHT_MODE_CHANNEL 8
//
//#define FLIGHT_MODE_1 RTL
//#define FLIGHT_MODE_2 RTL
//#define FLIGHT_MODE_3 FLY_BY_WIRE_A
//#define FLIGHT_MODE_4 FLY_BY_WIRE_A
//#define FLIGHT_MODE_5 MANUAL
//#define FLIGHT_MODE_6 MANUAL
//
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE OPTIONAL
// THROTTLE_FS_VALUE OPTIONAL
// THROTTLE_FAILSAFE_ACTION OPTIONAL
//
// The throttle failsafe allows you to configure a software failsafe activated
// by a setting on the throttle input channel (channel 3).
//
// This can be used to achieve a failsafe override on loss of radio control
// without having to sacrifice one of your FLIGHT_MODE settings, as the
// throttle failsafe overrides the switch-selected mode.
//
// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default
// is for it to be disabled.
//
// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value
// below which the failsafe engages. The default is 975ms, which is a very low
// throttle setting. Most transmitters will let you trim the manual throttle
// position up so that you cannot engage the failsafe with a regular stick movement.
//
// Configure your receiver's failsafe setting for the throttle channel to the
// absolute minimum, and use the ArduPilotMega_demo program to check that
// you cannot reach that value with the throttle control. Leave a margin of
// at least 50 microseconds between the lowest throttle setting and
// THROTTLE_FS_VALUE.
//
// The FAILSAFE_ACTION setting determines what APM will do when throttle failsafe
// mode is entered while flying in AUTO mode. This is important in order to avoid
// accidental failsafe behaviour when flying waypoints that take the aircraft
// temporarily out of radio range.
//
// If FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes,
// the aircraft will head for home in RTL mode. If the throttle channel moves
// back up, it will return to AUTO or LOITER mode.
//
// The default behaviour is to ignore throttle failsafe in AUTO and LOITER modes.
//
//#define THROTTLE_FAILSAFE DISABLED
//#define THROTTLE_FS_VALUE 975
//#define THROTTLE_FAILSAFE_ACTION 2
//
//////////////////////////////////////////////////////////////////////////////
// AUTO_TRIM OPTIONAL
//
// ArduPilot Mega can update its trim settings by looking at the
// radio inputs when switching out of MANUAL mode. This allows you to
// manually trim your aircraft before switching to an assisted mode, but it
// also means that you should avoid switching out of MANUAL while you have
// any control stick deflection.
//
// The default is to enable AUTO_TRIM.
//
//#define AUTO_TRIM ENABLED
//
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_REVERSE OPTIONAL
//
// A few speed controls require the throttle servo signal be reversed. Setting
// this to ENABLED will reverse the throttle output signal. Ensure that your
// throttle needs to be reversed by using the hardware failsafe and the
// ArduPilotMega_demo program before setting this option.
//
// The default is to not reverse the signal.
//
//#define THROTTLE_REVERSE DISABLED
//
//////////////////////////////////////////////////////////////////////////////
// ENABLE_STICK_MIXING OPTIONAL
//
// If this option is set to ENABLED, manual control inputs are are respected
// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.)
//
// The default is to enable stick mixing, allowing the pilot to take
// emergency action without switching modes.
//
//#define ENABLE_STICK_MIXING ENABLED
//
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_OUT DEBUG
//
// When debugging, it can be useful to disable the throttle output. Set
// this option to DISABLED to disable throttle output signals.
//
// The default is to not disable throttle output.
//
//#define THROTTLE_OUT ENABLED
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// STARTUP BEHAVIOUR
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GROUND_START_DELAY OPTIONAL
//
// If configured, inserts a delay between power-up and the beginning of IMU
// calibration during a ground start.
//
// Use this setting to give you time to position the aircraft horizontally
// for the IMU calibration.
//
// The default is to begin IMU calibration immediately at startup.
//
//#define GROUND_START_DELAY 0
//
//////////////////////////////////////////////////////////////////////////////
// ENABLE_AIR_START OPTIONAL
//
// If air start is disabled then you will get a ground start (including IMU
// calibration) every time the AP is powered up. This means that if you get
// a power glitch or reboot for some reason in the air, you will probably
// crash, but it prevents a lot of problems on the ground like unintentional
// motor start-ups, etc.
//
// If air start is enabled then you will get an air start at power up and a
// ground start will be performed if the speed is near zero when we get gps
// lock.
//
// The default is to disable air start.
//
//#define ENABLE_AIR_START 0
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Altitude measurement and control.
//
// AOA OPTIONAL
//
// The angle in 100ths of a degree that the nose of the aircraft should be
// raised from horizontal in level flight. The default is 1 degree.
//
//#define AOA 100 // note, 100ths of a degree
//
// ALT_EST_GAIN OPTIONAL
//
// The gain of the altitude estimation function; a lower number results
// in slower error correction and smoother output. The default is a
// reasonable starting point.
//
//#define ALT_EST_GAIN 0.01
//
// ALTITUDE_MIX OPTIONAL
//
// Configures the blend between GPS and pressure altitude.
// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc.
//
// The default is to use only GPS altitude.
//
//#define ALTITUDE_MIX 0
//
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_CRUISE OPTIONAL
//
// The speed in metres per second to maintain during cruise. The default
// is 10m/s, which is a conservative value suitable for relatively small,
// light aircraft.
//
//#define AIRSPEED_CRUISE 10
//
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control (also used for throttle "nudging" in AUTO)
//
// AIRSPEED_FBW_MIN OPTIONAL
// AIRSPEED_FBW_MAX OPTIONAL
//
// Airspeed corresponding to minimum and maximum throttle in Fly By Wire B mode.
// The defaults are 6 and 30 metres per second.
//
// AIRSPEED_FBW_MAX also sets the maximum airspeed that the cruise airspeed can be "nudged" to in AUTO mode when ENABLE_STICK_MIXING is set.
// In AUTO the cruise airspeed can be increased between AIRSPEED_CRUISE and AIRSPEED_FBW_MAX by positioning the throttle
// stick in the top 1/2 of its range. Throttle stick in the bottom 1/2 provide regular AUTO control.
//
// THROTTLE_ALT_P OPTIONAL
// THROTTLE_ALT_I OPTIONAL
// THROTTLE_ALT_D OPTIONAL
//
// P, I and D terms for the throttle control loop. Defaults are 0.32, 0, 0.
//
// THROTTLE_ALT_INT_MAX OPTIONAL
//
// Maximum throttle input due to the integral. Limits the throttle input
// due to persistent inability to maintain the commanded speed. Helps
// prevent the throttle from staying wide open when the control is reduced
// after a period at maxium speed.
// Default is 20 (20%).
//
//#define AIRSPEED_FBW_MIN 6
//#define AIRSPEED_FBW_MAX 30
//#define THROTTLE_ALT_P 0.32
//#define THROTTLE_ALT_I 0.0
//#define THROTTLE_ALT_D 0.0
//#define THROTTLE_ALT_INT_MAX 20
//
//////////////////////////////////////////////////////////////////////////////
// Throttle control
//
// THROTTLE_MIN OPTIONAL
//
// The minimum throttle setting to which the autopilot will reduce the
// throttle while descending. The default is zero, which is
// suitable for aircraft with a steady power-off glide. Increase this
// value if your aircraft needs throttle to maintain a stable descent in
// level flight.
//
// THROTTLE_CRUISE OPTIONAL
//
// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight.
// The default is 45%, which is reasonable for a modestly powered aircraft.
//
// THROTTLE_MAX OPTIONAL
//
// The maximum throttle setting the autopilot will apply. The default is 75%.
// Reduce this value if your aicraft is overpowered, or has complex flight
// characteristics at high throttle settings.
//
//#define THROTTLE_MIN 0
//#define THROTTLE_CRUISE 45
//#define THROTTLE_MAX 75
//
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
// HEAD_MAX OPTIONAL
//
// The maximum commanded bank angle in either direction.
// The default is 45 degrees. Decrease this value if your aircraft is not
// stable or has difficulty maintaining altitude in a steep bank.
//
// PITCH_MAX OPTIONAL
//
// The maximum commanded pitch up angle.
// The default is 15 degrees. Care should be taken not to set this value too
// large, as the aircraft may stall.
//
// PITCH_MIN
//
// The maximum commanded pitch down angle. Note that this value must be
// negative. The default is -25 degrees. Care should be taken not to set
// this value too large as it may result in overspeeding the aircraft.
//
//#define HEAD_MAX 45
//#define PITCH_MAX 15
//#define PITCH_MIN -25
//////////////////////////////////////////////////////////////////////////////
// Attitude control gains
//
// Tuning values for the attitude control PID loops.
//
// The P term is the primary tuning value. This determines how the control
// deflection varies in proportion to the required correction.
//
// The I term is used to help control surfaces settle. This value should
// normally be kept low.
//
// The D term is used to control overshoot. Avoid using or adjusting this
// term if you are not familiar with tuning PID loops. It should normally
// be zero for most aircraft.
//
// Note: When tuning these values, start with changes of no more than 25% at
// a time.
//
// SERVO_ROLL_P OPTIONAL
// SERVO_ROLL_I OPTIONAL
// SERVO_ROLL_D OPTIONAL
//
// P, I and D terms for roll control. Defaults are 0.4, 0, 0.
//
// SERVO_ROLL_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. crosstracking).
// Default is 5 degrees.
//
// ROLL_SLEW_LIMIT EXPERIMENTAL
//
// Limits the slew rate of the roll control in degrees per second. If zero,
// slew rate is not limited. Default is to not limit the roll control slew rate.
// (This feature is currently not implemented.)
//
// SERVO_PITCH_P OPTIONAL
// SERVO_PITCH_I OPTIONAL
// SERVO_PITCH_D OPTIONAL
//
// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0.
//
// SERVO_PITCH_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. native flight
// AoA). If you find this value is insufficient, consider adjusting the AOA
// parameter.
// Default is 5 degrees.
//
// PITCH_COMP OPTIONAL
//
// Adds pitch input to compensate for the loss of lift due to roll control.
// Default is 0.20 (20% of roll control also applied to pitch control).
//
// SERVO_YAW_P OPTIONAL
// SERVO_YAW_I OPTIONAL
// SERVO_YAW_D OPTIONAL
//
// P, I and D terms for the YAW control. Defaults are 0., 0., 0.
//
// SERVO_YAW_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. crosstracking).
// Default is 5 degrees.
//
// RUDDER_MIX OPTIONAL
//
// Roll to yaw mixing. This allows for co-ordinated turns.
// Default is 0.50 (50% of roll control also applied to yaw control.)
//
//#define SERVO_ROLL_P 0.4
//#define SERVO_ROLL_I 0.0
//#define SERVO_ROLL_D 0.0
//#define SERVO_ROLL_INT_MAX 5
//#define ROLL_SLEW_LIMIT 0
//#define SERVO_PITCH_P 0.6
//#define SERVO_PITCH_I 0.0
//#define SERVO_PITCH_D 0.0
//#define SERVO_PITCH_INT_MAX 5
//#define PITCH_COMP 0.2
//#define SERVO_YAW_P 0.0 // Default is zero. A suggested value if you want to use this parameter is 0.5
//#define SERVO_YAW_I 0.0
//#define SERVO_YAW_D 0.0
//#define SERVO_YAW_INT_MAX 5
//#define RUDDER_MIX 0.5
//
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
// Tuning values for the navigation control PID loops.
//
// The P term is the primary tuning value. This determines how the control
// deflection varies in proportion to the required correction.
//
// The I term is used to control drift.
//
// The D term is used to control overshoot. Avoid adjusting this term if
// you are not familiar with tuning PID loops.
//
// Note: When tuning these values, start with changes of no more than 25% at
// a time.
//
// NAV_ROLL_P OPTIONAL
// NAV_ROLL_I OPTIONAL
// NAV_ROLL_D OPTIONAL
//
// P, I and D terms for navigation control over roll, normally used for
// controlling the aircraft's course. The P term controls how aggressively
// the aircraft will bank to change or hold course.
// Defaults are 0.7, 0.0, 0.02.
//
// NAV_ROLL_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. crosstracking).
// Default is 5 degrees.
//
// NAV_PITCH_ASP_P OPTIONAL
// NAV_PITCH_ASP_I OPTIONAL
// NAV_PITCH_ASP_D OPTIONAL
//
// P, I and D terms for pitch adjustments made to maintain airspeed.
// Defaults are 0.65, 0, 0.
//
// NAV_PITCH_ASP_INT_MAX OPTIONAL
//
// Maximum pitch offset due to the integral. This limits the control
// output from being overdriven due to a persistent offset (eg. inability
// to maintain the programmed airspeed).
// Default is 5 degrees.
//
// NAV_PITCH_ALT_P OPTIONAL
// NAV_PITCH_ALT_I OPTIONAL
// NAV_PITCH_ALT_D OPTIONAL
//
// P, I and D terms for pitch adjustments made to maintain altitude.
// Defaults are 0.65, 0, 0.
//
// NAV_PITCH_ALT_INT_MAX OPTIONAL
//
// Maximum pitch offset due to the integral. This limits the control
// output from being overdriven due to a persistent offset (eg. inability
// to maintain the programmed altitude).
// Default is 5 degrees.
//
//#define NAV_ROLL_P 0.7
//#define NAV_ROLL_I 0.01
//#define NAV_ROLL_D 0.02
//#define NAV_ROLL_INT_MAX 5
//#define NAV_PITCH_ASP_P 0.65
//#define NAV_PITCH_ASP_I 0.0
//#define NAV_PITCH_ASP_D 0.0
//#define NAV_PITCH_ASP_INT_MAX 5
//#define NAV_PITCH_ALT_P 0.65
//#define NAV_PITCH_ALT_I 0.0
//#define NAV_PITCH_ALT_D 0.0
//#define NAV_PITCH_ALT_INT_MAX 5
//
//////////////////////////////////////////////////////////////////////////////
// Energy/Altitude control gains
//
// The Energy/altitude control system uses throttle input to control aircraft
// altitude.
//
// The P term is the primary tuning value. This determines how the throttle
// setting varies in proportion to the required correction.
//
// The I term is used to compensate for small offsets.
//
// The D term is used to control overshoot. Avoid adjusting this term if
// you are not familiar with tuning PID loops.
//
// THROTTLE_TE_P OPTIONAL
// THROTTLE_TE_I OPTIONAL
// THROTTLE_TE_D OPTIONAL
//
// P, I and D terms for throttle adjustments made to control altitude.
// Defaults are 0.5, 0, 0.
//
// THROTTLE_TE_INT_MAX OPTIONAL
//
// Maximum throttle input due to the integral term. This limits the
// throttle from being overdriven due to a persistent offset (e.g.
// inability to maintain the programmed altitude).
// Default is 20%.
//
// THROTTLE_SLEW_LIMIT OPTIONAL
//
// Limits the slew rate of the throttle, in percent per second. Helps
// avoid sudden throttle changes, which can destabilise the aircraft.
// A setting of zero disables the feature.
// Default is zero (disabled).
//
// P_TO_T OPTIONAL
//
// Pitch to throttle feed-forward gain. Used when AIRSPEED_SENSOR
// is DISABLED. Default is 2.5.
//
//#define THROTTLE_TE_P 0.50
//#define THROTTLE_TE_I 0.0
//#define THROTTLE_TE_D 0.0
//#define THROTTLE_TE_INT_MAX 20
//#define THROTTLE_SLEW_LIMIT 0
//#define P_TO_T 2.5
//
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
// XTRACK_GAIN OPTIONAL
//
// Crosstrack compensation in degrees per metre off track.
// Default value is 0.01 degrees per metre. Values lower than 0.001 will
// disable crosstrack compensation.
//
// XTRACK_ENTRY_ANGLE OPTIONAL
//
// Maximum angle used to correct for track following.
// Default value is 30 degrees.
//
//#define XTRACK_GAIN 10 // deg/m * 100
//#define XTRACK_ENTRY_ANGLE 3000 // deg * 100
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUGGING
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Subsystem test and debug.
//
// DEBUG_SUBSYSTEM DEBUG
//
// Selects a subsystem debug mode. Default is 0.
//
// 0 = no debug
// 1 = Debug the Radio input
// 2 = Radio Setup / Servo output
// 4 = Debug the GPS input
// 5 = Debug the GPS input - RAW OUTPUT
// 6 = Debug the IMU
// 7 = Debug the Control Switch
// 8 = Debug the Servo DIP switches
// 9 = Debug the Relay out
// 10 = Debug the Magnetometer Note - The magnetometer is not supported in V1.0
// 11 = Debug the ABS pressure sensor
// 12 = Debug the stored waypoints
// 13 = Debug the Throttle
// 14 = Debug the Radio Min Max
// 15 = Debug the EEPROM - Hex Dump
// 16 = XBee X-CTU Range and RSSI Test
// 17 = Debug IMU - raw gyro and accel outputs
// 20 = Debug Analog Sensors
//
//
//#define DEBUG_SUBSYSTEM 0
//
//////////////////////////////////////////////////////////////////////////////
// DEBUG_LEVEL DEBUG
//
// Selects the lowest level of debug messages passed to the telemetry system.
// Default is SEVERITY_LOW. May be one of:
//
// SEVERITY_LOW
// SEVERITY_MEDIUM
// SEVERITY_HIGH
// SEVERITY_CRITICAL
//
//#define DEBUG_LEVEL SEVERITY_LOW
//
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
// Each of these logging options may be set to ENABLED to enable, or DISABLED
// to disable the logging of the respective data.
//
// LOG_ATTITUDE_FAST DEBUG
//
// Logs basic attitude info to the dataflash at 50Hz (uses more space).
// Defaults to DISABLED.
//
// LOG_ATTITUDE_MED OPTIONAL
//
// Logs basic attitude info to the dataflash at 10Hz (uses less space than
// LOG_ATTITUDE_FAST). Defaults to ENABLED.
//
// LOG_GPS OPTIONAL
//
// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED.
//
// LOG_PM OPTIONAL
//
// Logs IMU performance monitoring info every 20 seconds.
// Defaults to DISABLED.
//
// LOG_CTUN OPTIONAL
//
// Logs control loop tuning info at 10 Hz. This information is useful for tuning
// servo control loop gain values. Defaults to DISABLED.
//
// LOG_NTUN OPTIONAL
//
// Logs navigation tuning info at 10 Hz. This information is useful for tuning
// navigation control loop gain values. Defaults to DISABLED.
//
// LOG_ MODE OPTIONAL
//
// Logs changes to the flight mode upon occurrence. Defaults to ENABLED.
//
// LOG_RAW DEBUG
//
// Logs raw accelerometer and gyro data at 50 Hz (uses more space).
// Defaults to DISABLED.
//
// LOG_CMD OPTIONAL
//
// Logs new commands when they process.
// Defaults to ENABLED.
//
//#define LOG_ATTITUDE_FAST DISABLED
//#define LOG_ATTITUDE_MED ENABLED
//#define LOG_GPS ENABLED
//#define LOG_PM ENABLED
//#define LOG_CTUN DISABLED
//#define LOG_NTUN DISABLED
//#define LOG_MODE ENABLED
//#define LOG_RAW DISABLED
//#define LOG_CMD ENABLED
//
//////////////////////////////////////////////////////////////////////////////
// Navigation defaults
//
// WP_RADIUS_DEFAULT OPTIONAL
//
// When the user performs a factory reset on the APM, set the waypoint radius
// (the radius from a target waypoint within which the APM will consider
// itself to have arrived at the waypoint) to this value in meters. This is
// mainly intended to allow users to start using the APM without running the
// WaypointWriter first.
//
// LOITER_RADIUS_DEFAULT OPTIONAL
//
// When the user performs a factory reset on the APM, set the loiter radius
// (the distance the APM will attempt to maintain from a waypoint while
// loitering) to this value in meters. This is mainly intended to allow
// users to start using the APM without running the WaypointWriter first.
//
//#define WP_RADIUS_DEFAULT 20
//#define LOITER_RADIUS_DEFAULT 60
//
//////////////////////////////////////////////////////////////////////////////
// Debugging interface
//
// DEBUG_PORT OPTIONAL
//
// The APM will periodically send messages reporting what it is doing; this
// variable determines to which serial port they will be sent. Port 0 is the
// USB serial port on the shield, port 3 is the telemetry port.
//
//#define DEBUG_PORT 0
//
//
// Do not remove - this is to discourage users from copying this file
// and using it as-is rather than editing their own.
//
#error You should not copy APM_Config.h.reference - make your own APM_Config.h file with just the options you need.

55
Tools/ArduTracker/APM_Config_mavlink_hil.h
View File

@ -1,55 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Enable Autopilot Flight Mode
#define FLIGHT_MODE_CHANNEL 8
#define FLIGHT_MODE_1 AUTO
#define FLIGHT_MODE_2 RTL
#define FLIGHT_MODE_3 FLY_BY_WIRE_A
#define FLIGHT_MODE_4 FLY_BY_WIRE_B
#define FLIGHT_MODE_5 STABILIZE
#define FLIGHT_MODE_6 MANUAL
// Hardware in the loop protocol
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
// HIL_MODE SELECTION
//
// Mavlink supports
// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
// 2. HIL_MODE_SENSORS: full sensor simulation
#define HIL_MODE HIL_MODE_ATTITUDE
// HIL_PORT SELCTION
//
// PORT 1
// If you would like to run telemetry communications for a groundstation
// while you are running hardware in the loop it is necessary to set
// HIL_PORT to 1. This uses the port that would have been used for the gps
// as the hardware in the loop port. You will have to solder
// headers onto the gps port connection on the apm
// and connect via an ftdi cable.
// The baud rate is set to 115200 in this mode.
//
// PORT 3
// If you don't require telemetry communication with a gcs while running
// hardware in the loop you may use the telemetry port as the hardware in
// the loop port.
// The buad rate is controlled by SERIAL3_BAUD in this mode.
#define HIL_PORT 1
// You can set your gps protocol here for your actual
// hardware and leave it without affecting the hardware
// in the loop simulation
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
// Ground control station comms
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PORT 3
// Sensors
// All sensors are supported in all modes.
// The magnetometer is not used in
// HIL_MODE_ATTITUDE but you may leave it
// enabled if you wish.
#define AIRSPEED_SENSOR ENABLED
#define MAGNETOMETER ENABLED

23
Tools/ArduTracker/APM_Config_xplane.h
View File

@ -1,23 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#define DEBUG_SUBSYSTEM 0
#define FLIGHT_MODE_CHANNEL 8
#define FLIGHT_MODE_1 AUTO
#define FLIGHT_MODE_2 RTL
#define FLIGHT_MODE_3 FLY_BY_WIRE_A
#define FLIGHT_MODE_4 FLY_BY_WIRE_B
#define FLIGHT_MODE_5 STABILIZE
#define FLIGHT_MODE_6 MANUAL
#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define GCS_PROTOCOL GCS_PROTOCOL_DEBUGTERMINAL
#define GCS_PORT 3
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED
#define AIRSPEED_SENSOR ENABLED

879
Tools/ArduTracker/ArduTracker.pde
View File

@ -1,879 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
ArduPilotMega (unstable development version)
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short
Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi
Please contribute your ideas!
This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_BinComm.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h>
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID library
#include <GCS_MAVLink.h> // MAVLink GCS definitions
// Configuration
#include "config.h"
// Local modules
#include "defines.h"
#include "global_data.h"
#include "GCS.h"
#include "HIL.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port
FastSerialPort3(Serial3); // Telemetry port
////////////////////////////////////////////////////////////////////////////////
// Sensors
////////////////////////////////////////////////////////////////////////////////
//
// There are three basic options related to flight sensor selection.
//
// - Normal flight mode. Real sensors are used.
// - HIL Attitude mode. Most sensors are disabled, as the HIL
// protocol supplies attitude information directly.
// - HIL Sensors mode. Synthetic sensors are configured that
// supply data from the simulation.
//
#if HIL_MODE == HIL_MODE_NONE
// real sensors
AP_ADC_ADS7844 adc;
APM_BMP085_Class pitot; //TODO: 'pitot' is not an appropriate name for a static pressure sensor
AP_Compass_HMC5843 compass;
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA gps(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF gps(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX gps(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK gps(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19
AP_GPS_MTK19 gps(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_NONE gps(NULL);
#else
#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL
#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
AP_ADC_HIL adc;
APM_BMP085_HIL_Class pitot;
AP_Compass_HIL compass;
AP_GPS_HIL gps(NULL);
#elif HIL_MODE == HIL_MODE_ATTITUDE
AP_DCM_HIL dcm;
AP_GPS_HIL gps(NULL);
#else
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
HIL_MAVLINK hil;
#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
HIL_XPLANE hil;
#endif // HIL PROTOCOL
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
AP_IMU imu(&adc,getAddress(PARAM_IMU_OFFSET_0));
AP_DCM dcm(&imu, &gps, &compass);
#endif
////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD
// create an instance of the standard GCS.
BinComm::MessageHandler GCS_MessageHandlers[] = {
{BinComm::MSG_ANY, receive_message, NULL},
{BinComm::MSG_NULL, NULL, NULL}
};
GCS_STANDARD gcs(GCS_MessageHandlers);
#elif GCS_PROTOCOL == GCS_PROTOCOL_LEGACY
GCS_LEGACY gcs;
#elif GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL
GCS_DEBUGTERMINAL gcs;
#elif GCS_PROTOCOL == GCS_PROTOCOL_XPLANE
GCS_XPLANE gcs; // Should become a HIL
#elif GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
GCS_MAVLINK gcs;
#else
// If we are not using a GCS, we need a stub that does nothing.
GCS_Class gcs;
#endif
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
byte control_mode = MANUAL;
boolean failsafe = false; // did our throttle dip below the failsafe value?
boolean ch3_failsafe = false;
byte crash_timer;
byte oldSwitchPosition; // for remembering the control mode switch
boolean reverse_switch = 1; // do we read the reversing switches after startup?
byte ground_start_count = 6; // have we achieved first lock and set Home?
int ground_start_avg; // 5 samples to avg speed for ground start
boolean ground_start = false; // have we started on the ground?
const char *comma = ",";
const char* flight_mode_strings[] = {
"Manual",
"Circle",
"Stabilize",
"",
"",
"FBW_A",
"FBW_B",
"",
"",
"",
"Auto",
"RTL",
"Loiter",
"Takeoff",
"Land"};
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
3 Throttle
4 Rudder (if we have ailerons)
5 Mode
6 TBD
7 TBD
8 TBD
*/
uint16_t radio_in[8]; // current values from the transmitter - microseconds
uint16_t radio_out[8]; // Send to the PWM library
int16_t servo_out[8]; // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100
uint16_t elevon1_trim = 1500; // TODO: handle in EEProm
uint16_t elevon2_trim = 1500;
uint16_t ch1_temp = 1500; // Used for elevon mixing
uint16_t ch2_temp = 1500;
int reverse_roll = 1;
int reverse_pitch = 1;
int reverse_rudder = 1;
byte mix_mode = 0; // 0 = normal , 1 = elevons
int reverse_elevons = 1;
int reverse_ch1_elevon = 1;
int reverse_ch2_elevon = 1;
// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon
float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
// PID controllers
PID pidServoRoll(getAddress(PARAM_RLL2SRV_P),PID::STORE_EEPROM_FLOAT);
PID pidServoPitch(getAddress(PARAM_PTCH2SRV_P),PID::STORE_EEPROM_FLOAT);
PID pidServoRudder(getAddress(PARAM_YW2SRV_P),PID::STORE_EEPROM_FLOAT);
PID pidNavRoll(getAddress(PARAM_HDNG2RLL_P),PID::STORE_EEPROM_FLOAT);
PID pidNavPitchAirspeed(getAddress(PARAM_ARSPD2PTCH_P),PID::STORE_EEPROM_FLOAT);
PID pidNavPitchAltitude(getAddress(PARAM_ALT2PTCH_P),PID::STORE_EEPROM_FLOAT);
PID pidTeThrottle(getAddress(PARAM_ENRGY2THR_P),PID::STORE_EEPROM_FLOAT);
PID pidAltitudeThrottle(getAddress(PARAM_ALT2THR_P),PID::STORE_EEPROM_FLOAT);
PID *pid_index[] = {
&pidServoRoll,
&pidServoPitch,
&pidServoRudder,
&pidNavRoll,
&pidNavPitchAirspeed,
&pidNavPitchAltitude,
&pidTeThrottle,
&pidAltitudeThrottle
};
// GPS variables
// -------------
const float t7 = 10000000.0; // used to scale values for EEPROM and flash memory storage
float scaleLongUp; // used to reverse longtitude scaling
float scaleLongDown; // used to reverse longtitude scaling
boolean GPS_light = false; // status of the GPS light
// Location & Navigation
// ---------------------
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
long hold_course = -1; // deg * 100 dir of plane
long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
byte command_must_index; // current command memory location
byte command_may_index; // current command memory location
byte command_must_ID; // current command ID
byte command_may_ID; // current command ID
//byte EEPROM_command // 1 = from the list, 0 = generated
// Airspeed
// --------
int airspeed; // m/s * 100
int airspeed_nudge = 0; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
float airspeed_error; // m/s * 100
long energy_error; // energy state error (kinetic + potential) for altitude hold
long airspeed_energy_error; // kinetic portion of energy error
// Location Errors
// ---------------
long bearing_error; // deg * 100 : 0 to 36000
long altitude_error; // meters * 100 we are off in altitude
float crosstrack_error; // meters we are off trackline
// Sensors
// --------
float airpressure_raw; // Airspeed Sensor - is a float to better handle filtering
int airpressure_offset; // analog air pressure sensor while still
int airpressure; // airspeed as a pressure value
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2, initialized above threshold for filter
float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3, initialized above threshold for filter
float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3+4, initialized above threshold for filter
// Pressure Sensor variables
unsigned long abs_press;
unsigned long abs_press_filt;
unsigned long abs_press_gnd;
int ground_temperature;
int temp_unfilt;
long ground_alt; // Ground altitude from gps at startup in centimeters
long press_alt; // Pressure altitude
// flight mode specific
// --------------------
boolean takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes.
boolean land_complete = false;
int landing_pitch; // pitch for landing set by commands
int takeoff_pitch;
int takeoff_altitude;
int landing_distance; // meters;
// Loiter management
// -----------------
long old_target_bearing; // deg * 100
int loiter_total; // deg : how many times to loiter * 360
int loiter_delta; // deg : how far we just turned
int loiter_sum; // deg : how far we have turned around a waypoint
long loiter_time; // millis : when we started LOITER mode
int loiter_time_max; // millis : how long to stay in LOITER mode
// these are the values for navigation control functions
// ----------------------------------------------------
long nav_roll; // deg * 100 : target roll angle
long nav_pitch; // deg * 100 : target pitch angle
long altitude_estimate; // for smoothing GPS output
int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
// Waypoints
// ---------
long wp_distance; // meters - distance between plane and next waypoint
long wp_totalDistance; // meters - distance between old and next waypoint
byte next_wp_index; // Current active command index
// repeating event control
// -----------------------
byte event_id; // what to do - see defines
long event_timer; // when the event was asked for in ms
int event_delay; // how long to delay the next firing of event in millis
int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
int event_value; // per command value, such as PWM for servos
int event_undo_value; // the value used to undo commands
byte repeat_forever;
byte undo_event; // counter for timing the undo
// delay command
// --------------
int delay_timeout = 0; // used to delay commands
long delay_start = 0; // used to delay commands
// 3D Location vectors
// -------------------
struct Location home; // home location
struct Location prev_WP; // last waypoint
struct Location current_loc; // current location
struct Location next_WP; // next waypoint
struct Location tell_command; // command for telemetry
struct Location next_command; // command preloaded
long target_altitude; // used for
long offset_altitude; // used for
boolean home_is_set = false; // Flag for if we have gps lock and have set the home location
// patch antenna variables
struct Location trackVehicle_loc; // vehicle location to track with antenna
// IMU variables
// -------------
float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
float COGX; // Course overground X axis
float COGY = 1; // Course overground Y axis
// Performance monitoring
// ----------------------
long perf_mon_timer;
float imu_health; // Metric based on accel gain deweighting
int G_Dt_max; // Max main loop cycle time in milliseconds
byte gyro_sat_count;
byte adc_constraints;
byte renorm_sqrt_count;
byte renorm_blowup_count;
int gps_fix_count;
byte gcs_messages_sent;
// GCS
// ---
char GCS_buffer[53];
char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
// System Timers
// --------------
unsigned long fast_loopTimer; // Time in miliseconds of main control loop
unsigned long fast_loopTimeStamp = 0; // Time Stamp when fast loop was complete
unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops
byte slow_loopCounter = 0;
byte superslow_loopCounter = 0;
unsigned long deltaMiliSeconds; // Delta Time in miliseconds
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
int mainLoop_count;
unsigned long elapsedTime; // for doing custom events
unsigned long GPS_timer;
float load; // % MCU cycles used
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
void setup() {
init_ardupilot();
}
void loop()
{
// We want this to execute at 50Hz if possible
// -------------------------------------------
if (millis()-fast_loopTimer > 19) {
deltaMiliSeconds = millis() - fast_loopTimer;
load = float(fast_loopTimeStamp - fast_loopTimer)/deltaMiliSeconds;
G_Dt = (float)deltaMiliSeconds / 1000.f;
fast_loopTimer = millis();
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// Execute the medium loop
// -----------------------
medium_loop();
if (millis()- perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT);
if (get(PARAM_LOG_BITMASK) & MASK_LOG_PM)
Log_Write_Performance();
resetPerfData();
}
}
fast_loopTimeStamp = millis();
}
}
void fast_loop()
{
// This is the fast loop - we want it to execute at 50Hz if possible
// -----------------------------------------------------------------
if (deltaMiliSeconds > G_Dt_max)
G_Dt_max = deltaMiliSeconds;
// Read radio
// ----------
read_radio();
// check for throtle failsafe condition
// ------------------------------------
//if (get(PARAM_THR_FAILSAFE))
//set_failsafe(ch3_failsafe);
// Read Airspeed
// -------------
# if AIRSPEED_SENSOR == 1 && HIL_MODE != HIL_MODE_ATTITUDE
//read_airspeed();
# endif
//dcm.update_DCM(G_Dt);
# if HIL_MODE == HIL_MODE_DISABLED
//if (get(PARAM_LOG_BITMASK) & MASK_LOG_ATTITUDE_FAST)
//Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
//if (get(PARAM_LOG_BITMASK) & MASK_LOG_RAW)
//Log_Write_Raw();
#endif // HIL_MODE
// altitude smoothing
// ------------------
//if (control_mode != FLY_BY_WIRE_B)
//calc_altitude_error();
// inertial navigation
// ------------------
#if INERTIAL_NAVIGATION == ENABLED
// TODO: implement inertial nav function
//inertialNavigation();
#endif
// custom code/exceptions for flight modes
// ---------------------------------------
//update_current_flight_mode();
// apply desired roll, pitch and yaw to the plane
// ----------------------------------------------
//if (control_mode > MANUAL)
//stabilize();
// write out the servo PWM values
// ------------------------------
set_servos_4();
// XXX is it appropriate to be doing the comms below on the fast loop?
#if HIL_MODE != HIL_MODE_DISABLED
// kick the HIL to process incoming sensor packets
hil.update();
// send out hil data
hil.send_message(MSG_SERVO_OUT);
//hil.send_message(MSG_ATTITUDE);
//hil.send_message(MSG_LOCATION);
//hil.send_message(MSG_AIRSPEED);
#endif
// kick the GCS to process uplink data
gcs.update();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(45,1000);
#endif
// XXX this should be absorbed into the above,
// or be a "GCS fast loop" interface
}
void medium_loop()
{
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
// This case deals with the GPS
//-------------------------------
case 0:
medium_loopCounter++;
update_GPS();
#if HIL_MODE != HIL_MODE_ATTITUDE && MAGNETOMETER == 1
//compass.read(); // Read magnetometer
//compass.calculate(dcm.roll,dcm.pitch); // Calculate heading
#endif
break;
// This case performs some navigation computations
//------------------------------------------------
case 1:
medium_loopCounter++;
if(gps.new_data){
dTnav = millis() - medium_loopTimer;
medium_loopTimer = millis();
}
// calculate the plane's desired bearing
// -------------------------------------
//navigate();
break;
// command processing
//------------------------------
case 2:
medium_loopCounter++;
// perform next command
// --------------------
//update_commands();
break;
// This case deals with sending high rate telemetry
//-------------------------------------------------
case 3:
medium_loopCounter++;
//if ((get(PARAM_LOG_BITMASK) & MASK_LOG_ATTITUDE_MED) && !(get(PARAM_LOG_BITMASK) & MASK_LOG_ATTITUDE_FAST))
//Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
#if HIL_MODE != HIL_MODE_ATTITUDE
//if (get(PARAM_LOG_BITMASK) & MASK_LOG_CTUN)
//Log_Write_Control_Tuning();
#endif
//if (get(PARAM_LOG_BITMASK) & MASK_LOG_NTUN)
//Log_Write_Nav_Tuning();
//if (get(PARAM_LOG_BITMASK) & MASK_LOG_GPS)
//Log_Write_GPS(gps.time, current_loc.lat, current_loc.lng, gps.altitude, current_loc.alt, (long) gps.ground_speed, gps.ground_course, gps.fix, gps.num_sats);
// XXX this should be a "GCS medium loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(5,45);
// send all requested output streams with rates requested
// between 5 and 45 Hz
#else
//gcs.send_message(MSG_ATTITUDE); // Sends attitude data
#endif
break;
// This case controls the slow loop
//---------------------------------
case 4:
medium_loopCounter=0;
slow_loop();
break;
}
}
void slow_loop()
{
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
switch (slow_loopCounter){
case 0:
slow_loopCounter++;
superslow_loopCounter++;
//if(superslow_loopCounter >=15) {
// keep track of what page is in use in the log
// *** We need to come up with a better scheme to handle this...
//eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage());
//superslow_loopCounter = 0;
//}
break;
case 1:
slow_loopCounter++;
// Read 3-position switch on radio
// -------------------------------
read_control_switch();
// Read Control Surfaces/Mix switches
// ----------------------------------
if(reverse_switch){
update_servo_switches();
}
// Read main battery voltage if hooked up - does not read the 5v from radio
// ------------------------------------------------------------------------
#if BATTERY_EVENT == 1
read_battery();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
// Read Baro pressure
// ------------------
//read_airpressure();
#endif
break;
case 2:
slow_loopCounter = 0;
//update_events();
// XXX this should be a "GCS slow loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,5);
// send all requested output streams with rates requested
// between 1 and 5 Hz
#else
//gcs.send_message(MSG_LOCATION);
#endif
// send a heartbeat
gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz
//but should be at 1 Hz, new loop timer?
// display load
gcs.send_message(MSG_CPU_LOAD, load*100);
break;
}
}
void update_GPS(void)
{
if(gps.status() == 0)
{
gps.init(); // reinitialize dead connections
return; // let it warm up while other stuff is running
}
gps.update();
update_GPS_light();
if (gps.new_data && gps.fix) {
GPS_timer = millis();
// XXX We should be sending GPS data off one of the regular loops so that we send
// no-GPS-fix data too
#if GCS_PROTOCOL != GCS_PROTOCOL_MAVLINK
gcs.send_message(MSG_LOCATION);
#endif
// for performance
// ---------------
gps_fix_count++;
if(ground_start_count > 1){
ground_start_count--;
ground_start_avg += gps.ground_speed;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
SendDebugln("!! bad loc");
ground_start_count = 5;
} else {
if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){
startup_ground();
if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
init_home();
} else if (ENABLE_AIR_START == 0) {
init_home();
}
ground_start_count = 0;
}
}
current_loc.lng = gps.longitude; // Lon * 10**7
current_loc.lat = gps.latitude; // Lat * 10**7
// XXX this is bogus; should just force get(PARAM_ALT_MIX) to zero for GPS_PROTOCOL_IMU
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = gps.altitude;
#else
current_loc.alt = ((1 - get(PARAM_ALT_MIX)) * gps.altitude) + (get(PARAM_ALT_MIX) * press_alt); // alt_MSL centimeters (meters * 100)
#endif
// Calculate new climb rate
add_altitude_data(millis()/100, gps.altitude/10);
COGX = cosf(ToRad(gps.ground_course/100.0));
COGY = sinf(ToRad(gps.ground_course/100.0));
}
}
void update_current_flight_mode(void)
{
if(control_mode == AUTO){
crash_checker();
switch(command_must_ID){
case CMD_TAKEOFF:
if (hold_course > -1) {
calc_nav_roll();
} else {
nav_roll = 0;
}
#if AIRSPEED_SENSOR == ENABLED
calc_nav_pitch();
if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch;
#else
nav_pitch = (long)((float)gps.ground_speed / (float)get(PARAM_TRIM_AIRSPEED) * (float)takeoff_pitch * 0.5);
nav_pitch = constrain(nav_pitch, 500l, (long)takeoff_pitch);
#endif
servo_out[CH_THROTTLE] = get(PARAM_THR_MAX); //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle
// What is the case for doing something else? Why wouldn't you want max throttle for TO?
// ******************************
break;
case CMD_LAND:
calc_nav_roll();
#if AIRSPEED_SENSOR == ENABLED
calc_nav_pitch();
calc_throttle();
#else
calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle
calc_throttle(); // throttle based on altitude error
nav_pitch = landing_pitch; // pitch held constant
#endif
if (land_complete) {
servo_out[CH_THROTTLE] = 0;
}
break;
default:
hold_course = -1;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
}
}else{
switch(control_mode){
case RTL:
case LOITER:
hold_course = -1;
crash_checker();
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case FLY_BY_WIRE_A:
// fake Navigation output using sticks
nav_roll = ((radio_in[CH_ROLL] - radio_trim(CH_ROLL)) *
get(PARAM_LIM_ROLL) * reverse_roll) / 350;
nav_pitch = ((radio_in[CH_PITCH] - radio_trim(CH_PITCH)) *
3500l * reverse_pitch) / 350;
nav_roll = constrain(nav_roll, -get(PARAM_LIM_ROLL), get(PARAM_LIM_ROLL));
nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority
break;
case FLY_BY_WIRE_B:
// fake Navigation output using sticks
// We use get(PARAM_PITCH_MIN) because its magnitude is
// normally greater than get(PARAM_get(PARAM_PITCH_MAX))
nav_roll = ((radio_in[CH_ROLL] - radio_trim(CH_ROLL))
* get(PARAM_LIM_ROLL) * reverse_roll) / 350;
altitude_error = ((radio_in[CH_PITCH] - radio_trim(CH_PITCH))
* get(PARAM_LIM_PITCH_MIN) * -reverse_pitch) / 350;
nav_roll = constrain(nav_roll, -get(PARAM_LIM_ROLL), get(PARAM_LIM_ROLL));
#if AIRSPEED_SENSOR == ENABLED
airspeed_error = ((int)(get(PARAM_ARSPD_FBW_MAX) -
get(PARAM_ARSPD_FBW_MIN)) *
servo_out[CH_THROTTLE]) +
((int)get(PARAM_ARSPD_FBW_MIN) * 100);
// Intermediate calculation - airspeed_error is just desired airspeed at this point
airspeed_energy_error = (long)(((long)airspeed_error *
(long)airspeed_error) -
((long)airspeed * (long)airspeed))/20000;
//Changed 0.00005f * to / 20000 to avoid floating point calculation
airspeed_error = (airspeed_error - airspeed);
#endif
calc_throttle();
calc_nav_pitch();
break;
case STABILIZE:
nav_roll = 0;
nav_pitch = 0;
// throttle is passthrough
break;
case CIRCLE:
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS
// ----------------------------------------------------
nav_roll = get(PARAM_LIM_ROLL) / 3;
nav_pitch = 0;
if (failsafe == true){
servo_out[CH_THROTTLE] = get(PARAM_TRIM_THROTTLE);
}
break;
case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
servo_out[CH_ROLL] = reverse_roll * (radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * 9;
servo_out[CH_PITCH] = reverse_pitch * (radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9;
servo_out[CH_RUDDER] = reverse_rudder * (radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9;
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
}
}
}

380
Tools/ArduTracker/Attitude.pde
View File

@ -1,380 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//****************************************************************
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************
void stabilize()
{
float ch1_inf = 1.0f;
float ch2_inf = 1.0f;
float ch4_inf = 1.0f;
#if AIRSPEED_SENSOR == ENABLED
float speed_scaler = STANDARD_SPEED_SQUARED / (airspeed * airspeed);
speed_scaler = constrain(speed_scaler, 0.11f, 9.0f);
#endif
#if AIRSPEED_SENSOR == DISABLED
float speed_scaler;
if (servo_out[CH_THROTTLE] > 0)
speed_scaler = 0.5f + (THROTTLE_CRUISE / servo_out[CH_THROTTLE] / 2.0f); // First order taylor expansion of square root
// Should maybe be to the 2/7 power, but we aren't goint to implement that...
else
speed_scaler = 1.67f;
speed_scaler = constrain(speed_scaler, 0.6f, 1.67f); // This case is constrained tighter as we don't have real speed info
#endif
if(crash_timer > 0){
nav_roll = 0;
}
// For Testing Only
// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10;
// Serial.print(" roll_sensor ");
// Serial.print(roll_sensor,DEC);
// Calculate dersired servo output for the roll
// ---------------------------------------------
servo_out[CH_ROLL] = pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), deltaMiliSeconds, speed_scaler);
servo_out[CH_PITCH] = pidServoPitch.get_pid((nav_pitch + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) - (dcm.pitch_sensor - get(PARAM_TRIM_PITCH))), deltaMiliSeconds, speed_scaler);
//Serial.print(" servo_out[CH_ROLL] ");
//Serial.print(servo_out[CH_ROLL],DEC);
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) {
ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim(CH_ROLL);
ch1_inf = fabsf(ch1_inf);
ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
ch2_inf = (float)radio_in[CH_PITCH] - radio_trim(CH_PITCH);
ch2_inf = fabsf(ch2_inf);
ch2_inf = min(ch2_inf, 400.0);
ch2_inf = ((400.0 - ch2_inf) /400.0);
// scale the sensor input based on the stick input
// -----------------------------------------------
servo_out[CH_ROLL] *= ch1_inf;
servo_out[CH_PITCH] *= ch2_inf;
// Mix in stick inputs
// -------------------
servo_out[CH_ROLL] += reverse_roll * (radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * 9;
servo_out[CH_PITCH] += reverse_pitch * (radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9;
//Serial.print(" servo_out[CH_ROLL] ");
//Serial.println(servo_out[CH_ROLL],DEC);
}
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
// important for steering on the ground during landing
// -----------------------------------------------
if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) {
ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim(CH_RUDDER);
ch4_inf = fabsf(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
// Apply output to Rudder
// ----------------------
calc_nav_yaw(speed_scaler);
servo_out[CH_RUDDER] *= ch4_inf;
servo_out[CH_RUDDER] += reverse_rudder * (radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9;
// Call slew rate limiter if used
// ------------------------------
//#if(ROLL_SLEW_LIMIT != 0)
// servo_out[CH_ROLL] = roll_slew_limit(servo_out[CH_ROLL]);
//#endif
}
void crash_checker()
{
if(dcm.pitch_sensor < -4500){
crash_timer = 255;
}
if(crash_timer > 0)
crash_timer--;
}
#if AIRSPEED_SENSOR == DISABLED
void calc_throttle()
{
int throttle_target = get(PARAM_TRIM_THROTTLE) + throttle_nudge;
// no airspeed sensor, we use nav pitch to determine the proper throttle output
// AUTO, RTL, etc
// ---------------------------------------------------------------------------
if (nav_pitch >= 0) {
servo_out[CH_THROTTLE] = throttle_target + (get(PARAM_THR_MAX) - throttle_target) * nav_pitch / get(PARAM_LIM_PITCH_MAX);
} else {
servo_out[CH_THROTTLE] = throttle_target - (throttle_target - get(PARAM_THR_MIN)) * nav_pitch / get(PARAM_LIM_PITCH_MIN);
}
servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], get(PARAM_THR_MIN), get(PARAM_THR_MAX));
}
#endif
#if AIRSPEED_SENSOR == ENABLED
void calc_throttle()
{
// throttle control with airspeed compensation
// -------------------------------------------
energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;
// positive energy errors make the throttle go higher
servo_out[CH_THROTTLE] = get(PARAM_TRIM_THROTTLE) + pidTeThrottle.get_pid(energy_error, dTnav);
servo_out[CH_THROTTLE] = max(servo_out[CH_THROTTLE], 0);
// are we going too slow? up the throttle to get some more groundspeed
// move into PID loop in the future
// lower the contstant value to limit the effect : 50 = default
//JASON - We really should change this to act on airspeed in this case. Let's visit about it....
/*int gs_boost = 30 * (1.0 - ((float)gps.ground_speed / (float)airspeed_cruise));
gs_boost = max(0,gs_boost);
servo_out[CH_THROTTLE] += gs_boost;*/
servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE],
get(PARAM_THR_MIN), get(PARAM_THR_MAX));
}
#endif
/*****************************************
* Calculate desired roll/pitch/yaw angles (in medium freq loop)
*****************************************/
// Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ----------------------------------------------------------------------------------------
void calc_nav_yaw(float speed_scaler)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
Vector3f temp = imu.get_accel();
long error = -temp.y;
// Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero)
servo_out[CH_RUDDER] = get(PARAM_KFF_RDDRMIX) * servo_out[CH_ROLL] + pidServoRudder.get_pid(error, deltaMiliSeconds, speed_scaler);
#else
servo_out[CH_RUDDER] = get(PARAM_KFF_RDDRMIX) * servo_out[CH_ROLL];
// XXX probably need something here based on heading
#endif
}
void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
#if AIRSPEED_SENSOR == ENABLED
nav_pitch = -pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
#else
nav_pitch = pidNavPitchAltitude.get_pid(altitude_error, dTnav);
#endif
nav_pitch = constrain(nav_pitch, get(PARAM_LIM_PITCH_MIN), get(PARAM_LIM_PITCH_MAX));
}
void calc_nav_roll()
{
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
// This does not make provisions for wind speed in excess of airframe speed
nav_gain_scaler = (float)gps.ground_speed / (STANDARD_SPEED * 100.0);
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
// negative error = left turn
// positive error = right turn
// Calculate the required roll of the plane
// ----------------------------------------
nav_roll = pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
nav_roll = constrain(nav_roll,-get(PARAM_LIM_ROLL), get(PARAM_LIM_ROLL));
}
/*****************************************
* Roll servo slew limit
*****************************************/
/*
float roll_slew_limit(float servo)
{
static float last;
float temp = constrain(servo, last-ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f);
last = servo;
return temp;
}*/
/*****************************************
* Throttle slew limit
*****************************************/
/*float throttle_slew_limit(float throttle)
{
static float last;
float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f);
last = throttle;
return temp;
}
*/
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
void reset_I(void)
{
pidNavRoll.reset_I();
pidNavPitchAirspeed.reset_I();
pidNavPitchAltitude.reset_I();
pidTeThrottle.reset_I();
pidAltitudeThrottle.reset_I();
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void set_servos_4(void)
{
if(control_mode == MANUAL){
// do a direct pass through of radio values
for(int y=0; y<4; y++) {
radio_out[y] = radio_in[y];
}
} else {
// Patch Antenna Control Code
float phi, theta; //,servo_phi, servo_theta;
float x1,x2,y1,y2,x,y,r,z;
y1 = 110600*current_loc.lat/t7;
x1 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(current_loc.lng/t7);
y2 = 110600*trackVehicle_loc.lat/t7;
x2 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(trackVehicle_loc.lng/t7);
x = abs(x2 - x1);
y = abs(y2 - y1);
r = sqrtf(x*x+y*y);
z = trackVehicle_loc.alt/100.0f - current_loc.alt;
phi = (atanf(z/r)*180/PI);
theta = (atanf(x/y)*180/PI);
// Check to see which quadrant of the angle
if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng)
{
theta = abs(theta);
}
else if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng <= current_loc.lng)
{
theta = 360 - abs(theta);
}
else if (trackVehicle_loc.lat <= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng)
{
theta = 180 - abs(theta);
}
else if (trackVehicle_loc.lat <= current_loc.lat && trackVehicle_loc.lng <= current_loc.lng)
{
theta = 180 + abs(theta);
}
// Calibration of the top servo
//servo_phi = (91*phi + 7650)/9;
// Calibration of the bottom servo
//servo_theta = (2*theta + 5940)/3;
Serial.print("target lat: "); Serial.println(current_loc.lat);
Serial.print("tracker lat: "); Serial.println(trackVehicle_loc.lat);
Serial.print("phi: "); Serial.println(phi);
Serial.print("theta: "); Serial.println(theta);
// Outputing to the servos
servo_out[CH_ROLL] = 10000*phi/90.0f;
servo_out[CH_PITCH] = 10000*theta/360.0f;
servo_out[CH_RUDDER] = 0;
servo_out[CH_THROTTLE] = 0;
radio_out[CH_ROLL] = radio_trim(CH_ROLL) + (reverse_roll * ((float)servo_out[CH_ROLL] * 0.11111));
radio_out[CH_PITCH] = radio_trim(CH_PITCH) + (reverse_pitch * ((float)servo_out[CH_PITCH] * 0.11111));
radio_out[CH_RUDDER] = 0;
radio_out[CH_THROTTLE] = 0;
/*
if (mix_mode == 0){
radio_out[CH_ROLL] = radio_trim(CH_ROLL) + (reverse_roll * ((float)servo_out[CH_ROLL] * 0.11111));
radio_out[CH_PITCH] = radio_trim(CH_PITCH) + (reverse_pitch * ((float)servo_out[CH_PITCH] * 0.11111));
radio_out[CH_RUDDER] = radio_trim(CH_RUDDER) + (reverse_rudder * ((float)servo_out[CH_RUDDER] * 0.11111));
}else{
//Elevon mode
float ch1;
float ch2;
ch1 = reverse_elevons * (servo_out[CH_PITCH] - servo_out[CH_ROLL]);
ch2 = servo_out[CH_PITCH] + servo_out[CH_ROLL];
radio_out[CH_ROLL] = elevon1_trim + (reverse_ch1_elevon * (ch1 * 0.11111));
radio_out[CH_PITCH] = elevon2_trim + (reverse_ch2_elevon * (ch2 * 0.11111));
}
#if THROTTLE_OUT == 0
radio_out[CH_THROTTLE] = 0;
#endif
// convert 0 to 100% into PWM
servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100);
radio_out[CH_THROTTLE] = servo_out[CH_THROTTLE] * ((radio_max(CH_THROTTLE) - radio_min(CH_THROTTLE)) / 100);
radio_out[CH_THROTTLE] += radio_min(CH_THROTTLE);
//Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938
#if THROTTLE_REVERSE == 1
radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
#endif
*/
}
// send values to the PWM timers for output
// ----------------------------------------
for(int y=0; y<4; y++) {
APM_RC.OutputCh(y, radio_out[y]); // send to Servos
}
}
void demo_servos(byte i) {
while(i > 0){
gcs.send_text(SEVERITY_LOW,"Demo Servos!");
APM_RC.OutputCh(1, 1400);
delay(400);
APM_RC.OutputCh(1, 1600);
delay(200);
APM_RC.OutputCh(1, 1500);
delay(400);
i--;
}
}
int readOutputCh(unsigned char ch)
{
int pwm;
switch(ch)
{
case 0: pwm = OCR5B; break; // ch0
case 1: pwm = OCR5C; break; // ch1
case 2: pwm = OCR1B; break; // ch2
case 3: pwm = OCR1C; break; // ch3
case 4: pwm = OCR4C; break; // ch4
case 5: pwm = OCR4B; break; // ch5
case 6: pwm = OCR3C; break; // ch6
case 7: pwm = OCR3B; break; // ch7
case 8: pwm = OCR5A; break; // ch8, PL3
case 9: pwm = OCR1A; break; // ch9, PB5
case 10: pwm = OCR3A; break; // ch10, PE3
}
pwm >>= 1; // pwm / 2;
return pwm;
}

266
Tools/ArduTracker/GCS.h
View File

@ -1,266 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file GCS.h
/// @brief Interface definition for the various Ground Control System protocols.
#ifndef __GCS_H
#define __GCS_H
#include <BetterStream.h>
#include <AP_Common.h>
#include <APM_BinComm.h>
#include <GCS_MAVLink.h>
#include <GPS.h>
#include <Stream.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(BetterStream *port) { _port = port; }
/// 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(uint8_t id, int32_t param = 0) {}
/// Send a text message.
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const char *str) {}
/// Send acknowledgement for a message.
///
/// @param id The message ID being acknowledged.
/// @param sum1 Checksum byte 1 from the message being acked.
/// @param sum2 Checksum byte 2 from the message being acked.
///
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {}
/// Emit an update of the "current" waypoints, often previous, current and
/// next.
///
void print_current_waypoints() {}
//
// The following interfaces are not currently implemented as their counterparts
// are not called in the mainline code. XXX ripe for re-specification.
//
/// Send a text message with printf-style formatting.
///
/// @param severity A value describing the importance of the message.
/// @param fmt The format string to send.
/// @param ... Additional arguments to the format string.
///
// void send_message(uint8_t severity, const char *fmt, ...) {}
/// Log a waypoint
///
/// @param wp The waypoint to log.
/// @param index The index of the waypoint.
// void print_waypoint(struct Location *wp, uint8_t index) {}
// test if frequency within range requested for loop
// used by data_stream_send
static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
{
if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
else return false;
}
// send streams which match frequency range
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
protected:
/// The stream we are communicating over
BetterStream *_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_STANDARD
/// @brief The default APM GCS protocol
///
#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD
class GCS_STANDARD : public GCS_Class
{
public:
GCS_STANDARD(BinComm::MessageHandler GCS_MessageHandlers[]);
void update(void);
void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
BinComm & getBinComm(void) { return _binComm; }
private:
BinComm _binComm;
};
#endif // GCS_PROTOCOL_STANDARD
///
/// @class GCS_MAVLINK
/// @brief The mavlink protocol for qgroundcontrol
///
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK();
void update(void);
void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
private:
void handleMessage(mavlink_message_t * msg);
mavlink_channel_t chan;
uint16_t packet_drops;
uint16_t rawSensorStreamRate;
uint16_t attitudeStreamRate;
uint16_t positionStreamRate;
uint16_t rawControllerStreamRate;
uint16_t rcStreamRate;
uint16_t extraStreamRate[3];
};
#endif // GCS_PROTOCOL_MAVLINK
///
/// @class GCS_LEGACY
/// @brief Support for the legacy Ardupilot GCS
///
#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY
class GCS_LEGACY : public GCS_Class
{
public:
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
private:
void print_attitude(void);
void print_control_mode(void);
void print_position(void);
void print_waypoint(struct Location *cmd,byte index);
};
#endif // GCS_PROTOTOL_LEGACY
///
/// @class GCS_DEBUGTERMINAL
/// @brief A GCS driver with an interactive management interface
///
#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL
class GCS_DEBUGTERMINAL : public GCS_Class
{
public:
GCS_DEBUGTERMINAL() :
report_command(1)
{}
void update(void);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void print_current_waypoints(void);
void print_commands();
void print_PID(long PID_error, long dt, float scaler, float derivative, float integrator, float last_error);
private:
char gcsinputbuffer[120]; // read buffer
byte bufferidx;
// Reporting flags
byte report_heartbeat;
byte report_attitude;
byte report_location;
byte report_command;
byte report_severity;
byte first_location;
byte report_cpu_load;
void processgcsinput(char c);
void run_debugt_command(char *buf);
static int strmatch(char* s1, const char* s2);
static long readfloat(char* s, unsigned char* i);
void process_set_cmd(char *buffer, int bufferlen);
static int get_pid_offset_name(char *buffer, int *offset, unsigned char *length);
void print_rlocation(Location *wp);
void print_error(const char *msg);
void print_position(void);
void print_attitude(void);
void print_new_wp_info();
void print_control_mode(void);
void print_tuning(void);
void print_command(struct Location *cmd, byte index);
void print_command_id(byte id);
void print_gain(unsigned char g);
void print_gains();
void print_gain_keyword_error();
void print_commands(unsigned char i1, unsigned char i2);
};
#endif // GCS_PROTOCOL_DEBUGTERMINAL
///
/// @class GCS_XPLANE
/// @brief GCS driver for HIL operation with the X-plane simulator
///
#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE
class GCS_XPLANE : public GCS_Class
{
public:
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void print_current_waypoints(void);
private:
void print_control_mode(void);
void print_waypoint(struct Location *wp, uint8_t index);
void print_waypoints(void);
};
#endif // GCS_PROTOCOL_XPLANE
#endif // __GCS_H

157
Tools/ArduTracker/GCS_Ardupilot.pde
View File

@ -1,157 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
///
/// @file GCS_Ardupilot.pde
/// @brief GCS driver for the legacy Ardupilot GCS protocol.
///
#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY
/*
Message Prefixes
!!! Position Low rate telemetry
+++ Attitude High rate telemetry
### Mode Change in control mode
%%% Waypoint Current and previous waypoints
XXX Alert Text alert - NOTE: Alert message generation is not localized to a function
PPP IMU Performance Sent every 20 seconds for performance monitoring
Message Suffix
*** All messages use this suffix
*/
void
GCS_LEGACY::send_text(uint8_t severity, const char *str)
{
if(severity >= DEBUG_LEVEL){
_port->print("MSG: ");
_port->println(str);
}
}
void
GCS_LEGACY::send_message(uint8_t id, uint32_t param)
{
switch(id) {
case MSG_ATTITUDE: // ** Attitude message
print_attitude();
break;
case MSG_LOCATION: // ** Location/GPS message
print_position();
break;
case MSG_MODE_CHANGE:
case MSG_HEARTBEAT: // ** Location/GPS message
print_control_mode();
break;
case MSG_CPU_LOAD:
//TODO: replace with appropriate message
_port->printf_P(PSTR("MSG: load %ld%%\n"), param);
break;
//case MSG_PERF_REPORT:
// printPerfData();
}
}
void
GCS_LEGACY::print_attitude(void)
{
_port->print("+++");
_port->print("ASP:");
_port->print((int)airspeed / 100, DEC);
_port->print(",THH:");
_port->print(servo_out[CH_THROTTLE], DEC);
_port->print (",RLL:");
_port->print(roll_sensor / 100, DEC);
_port->print (",PCH:");
_port->print(pitch_sensor / 100, DEC);
_port->println(",***");
}
void
GCS_LEGACY::print_control_mode(void)
{
switch (control_mode){
case MANUAL:
_port->println("###MANUAL***");
break;
case STABILIZE:
_port->println("###STABILIZE***");
break;
case CIRCLE:
_port->println("###CIRCLE***");
break;
case FLY_BY_WIRE_A:
_port->println("###FBW A***");
break;
case FLY_BY_WIRE_B:
_port->println("###FBW B***");
break;
case AUTO:
_port->println("###AUTO***");
break;
case RTL:
_port->println("###RTL***");
break;
case LOITER:
_port->println("###LOITER***");
break;
case TAKEOFF:
_port->println("##TAKEOFF***");
break;
case LAND:
_port->println("##LAND***");
break;
}
}
void
GCS_LEGACY::print_position(void)
{
_port->print("!!!");
_port->print("LAT:");
_port->print(current_loc.lat/10,DEC);
_port->print(",LON:");
_port->print(current_loc.lng/10,DEC); //wp_current_lat
_port->print(",SPD:");
_port->print(gps.ground_speed/100,DEC);
_port->print(",CRT:");
_port->print(climb_rate,DEC);
_port->print(",ALT:");
_port->print(current_loc.alt/100,DEC);
_port->print(",ALH:");
_port->print(next_WP.alt/100,DEC);
_port->print(",CRS:");
_port->print(yaw_sensor/100,DEC);
_port->print(",BER:");
_port->print(target_bearing/100,DEC);
_port->print(",WPN:");
_port->print(get(PARAM_WP_INDEX),DEC);//Actually is the waypoint.
_port->print(",DST:");
_port->print(wp_distance,DEC);
_port->print(",BTV:");
_port->print(battery_voltage,DEC);
_port->print(",RSP:");
_port->print(servo_out[CH_ROLL]/100,DEC);
_port->print(",TOW:");
_port->print(gps.time);
_port->println(",***");
}
void
GCS_LEGACY::print_waypoint(struct Location *cmd,byte index)
{
_port->print("command #: ");
_port->print(index, DEC);
_port->print(" id: ");
_port->print(cmd->id,DEC);
_port->print(" p1: ");
_port->print(cmd->p1,DEC);
_port->print(" p2: ");
_port->print(cmd->alt,DEC);
_port->print(" p3: ");
_port->print(cmd->lat,DEC);
_port->print(" p4: ");
_port->println(cmd->lng,DEC);
}
#endif // GCS_PROTOCOL_LEGACY

1622
Tools/ArduTracker/GCS_DebugTerminal.pde
View File

File diff suppressed because it is too large Load Diff

610
Tools/ArduTracker/GCS_Mavlink.pde
View File

@ -1,610 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
#include "Mavlink_Common.h"
GCS_MAVLINK::GCS_MAVLINK() :
packet_drops(0)
{
}
void
GCS_MAVLINK::init(BetterStream * port)
{
GCS_Class::init(port);
mavlink_comm_1_port = port;
chan = MAVLINK_COMM_1;
}
void
GCS_MAVLINK::update(void)
{
// recieve new packets
mavlink_message_t msg;
mavlink_status_t status;
// process received bytes
while(comm_get_available(chan))
{
uint8_t c = comm_receive_ch(chan);
// Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
}
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
mavlink_queued_send(chan);
// send parameters communication_queued_send(chan);
// stop waypoint sending if timeout
if (global_data.waypoint_sending &&
millis() - global_data.waypoint_timelast_send >
global_data.waypoint_send_timeout)
{
send_text(SEVERITY_LOW,"waypoint send timeout");
global_data.waypoint_sending = false;
}
// stop waypoint receiving if timeout
if (global_data.waypoint_receiving &&
millis() - global_data.waypoint_timelast_receive >
global_data.waypoint_receive_timeout)
{
send_text(SEVERITY_LOW,"waypoint receive timeout");
global_data.waypoint_receiving = false;
}
}
void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
{
if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax))
send_message(MSG_RAW_IMU);
if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax))
send_message(MSG_EXTENDED_STATUS);
if (freqLoopMatch(global_data.streamRateRCChannels,freqMin,freqMax))
send_message(MSG_RADIO_OUT);
if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax))
send_message(MSG_SERVO_OUT);
//if (freqLoopMatch(global_data.streamRateRawSensorFusion,freqMin,freqMax))
if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax))
send_message(MSG_LOCATION);
if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax))
send_message(MSG_GPS_STATUS);
if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax))
send_message(MSG_CURRENT_WAYPOINT);
if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax))
{
send_message(MSG_LOCAL_LOCATION);
send_message(MSG_ATTITUDE);
}
}
void
GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
{
mavlink_send_message(chan,id,param,packet_drops);
}
void
GCS_MAVLINK::send_text(uint8_t severity, const char *str)
{
mavlink_send_text(chan,severity,str);
}
void
GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
mavlink_acknowledge(chan,id,sum1,sum2);
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_GLOBAL_POSITION:
{
// decode
mavlink_global_position_t packet;
mavlink_msg_global_position_decode(msg, &packet);
//if (mavlink_check_target(packet.target_system,packet.target_component)) break;
trackVehicle_loc.id = 0;
trackVehicle_loc.p1 = 0;
trackVehicle_loc.alt = packet.alt;
trackVehicle_loc.lat = packet.lat;
trackVehicle_loc.lng = packet.lon;
Serial.println("received global position packet");
}
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;
int 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:
global_data.streamRateRawSensors = freq;
global_data.streamRateExtendedStatus = freq;
global_data.streamRateRCChannels = freq;
global_data.streamRateRawController = freq;
global_data.streamRateRawSensorFusion = freq;
global_data.streamRatePosition = freq;
global_data.streamRateExtra1 = freq;
global_data.streamRateExtra2 = freq;
global_data.streamRateExtra3 = freq;
break;
case MAV_DATA_STREAM_RAW_SENSORS:
global_data.streamRateRawSensors = freq;
break;
case MAV_DATA_STREAM_EXTENDED_STATUS:
global_data.streamRateExtendedStatus = freq;
break;
case MAV_DATA_STREAM_RC_CHANNELS:
global_data.streamRateRCChannels = freq;
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
global_data.streamRateRawController = freq;
break;
case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
global_data.streamRateRawSensorFusion = freq;
break;
case MAV_DATA_STREAM_POSITION:
global_data.streamRatePosition = freq;
break;
case MAV_DATA_STREAM_EXTRA1:
global_data.streamRateExtra1 = freq;
break;
case MAV_DATA_STREAM_EXTRA2:
global_data.streamRateExtra2 = freq;
break;
case MAV_DATA_STREAM_EXTRA3:
global_data.streamRateExtra3 = freq;
break;
default:
break;
}
}
case MAVLINK_MSG_ID_ACTION:
{
// decode
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (mavlink_check_target(packet.target,packet.target_component)) break;
// do action
gcs.send_text(SEVERITY_LOW,"action received");
switch(packet.action)
{
case MAV_ACTION_LAUNCH:
set_mode(TAKEOFF);
break;
case MAV_ACTION_RETURN:
set_mode(RTL);
break;
case MAV_ACTION_EMCY_LAND:
set_mode(LAND);
break;
case MAV_ACTION_HALT:
loiter_at_location();
break;
// can't implement due to APM configuration
// just setting to manual to be safe
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
set_mode(MANUAL);
break;
case MAV_ACTION_CONTINUE:
process_next_command();
break;
case MAV_ACTION_SET_MANUAL:
set_mode(MANUAL);
break;
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
break;
case MAV_ACTION_STORAGE_READ:
//read_EEPROM_startup();
//read_EEPROM_airstart_critical();
//read_command_index();
//read_EEPROM_flight_modes();
break;
case MAV_ACTION_STORAGE_WRITE:
//save_EEPROM_trims();
//save_EEPROM_waypoint_info();
//save_EEPROM_gains();
//save_command_index();
//save_pressure_data();
//save_EEPROM_radio_minmax();
//save_user_configs();
//save_EEPROM_flight_modes();
break;
case MAV_ACTION_CALIBRATE_RC: break;
trim_radio();
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_IMU_ground();
break;
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
case MAV_ACTION_TAKEOFF:
set_mode(TAKEOFF);
break;
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
break;
case MAV_ACTION_LAND:
set_mode(LAND);
break;
case MAV_ACTION_LOITER:
set_mode(LOITER);
break;
default: break;
}
}
break;
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
//send_text(SEVERITY_LOW,"waypoint request list");
// decode
mavlink_waypoint_request_list_t packet;
mavlink_msg_waypoint_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// Start sending waypoints
mavlink_msg_waypoint_count_send(chan,msg->sysid,
msg->compid,get(PARAM_WP_TOTAL));
global_data.waypoint_timelast_send = millis();
global_data.waypoint_sending = true;
global_data.waypoint_receiving = false;
global_data.waypoint_dest_sysid = msg->sysid;
global_data.waypoint_dest_compid = msg->compid;
}
break;
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
//send_text(SEVERITY_LOW,"waypoint request");
// Check if sending waypiont
if (!global_data.waypoint_sending) break;
// decode
mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// send waypoint
tell_command = get_wp_with_index(packet.seq);
// set frame of waypoint
uint8_t frame = MAV_FRAME_GLOBAL; // reference frame
uint8_t action = MAV_ACTION_NAVIGATE; // action
uint8_t orbit_direction = 0; // clockwise(0), counter-clockwise(1)
float orbit = 0; // loiter radius
float param1 = 0, param2 = 0;
switch(tell_command.id)
{
case CMD_WAYPOINT: // navigate
action = MAV_ACTION_NAVIGATE; // action
break;
case CMD_LOITER_TIME: // loiter
orbit = get(PARAM_WP_RADIUS); // XXX setting loiter radius as waypoint acceptance radius
action = MAV_ACTION_LOITER; // action
param1 = get(PARAM_WP_RADIUS);
param2 = tell_command.p1*100; // loiter time
break;
case CMD_TAKEOFF: // takeoff
action = MAV_ACTION_TAKEOFF;
break;
case CMD_LAND: // land
action = MAV_ACTION_LAND;
break;
defaut:
gcs.send_text(SEVERITY_LOW,"command not handled");
break;
}
// time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == get(PARAM_WP_INDEX)) current = 1;
float yaw_dir = 0; // yaw orientation in radians, 0 = north XXX: what does this do?
uint8_t autocontinue = 1; // 1 (true), 0 (false)
float x = tell_command.lng/1.0e7; // local (x), global (longitude)
float y = tell_command.lat/1.0e7; // local (y), global (latitude)
float z = tell_command.alt/1.0e2; // local (z), global (altitude)
// note XXX: documented x,y,z order does not match with gps raw
mavlink_msg_waypoint_send(chan,msg->sysid,
msg->compid,packet.seq,frame,action,
orbit,orbit_direction,param1,param2,current,x,y,z,yaw_dir,autocontinue);
// update last waypoint comm stamp
global_data.waypoint_timelast_send = millis();
}
break;
case MAVLINK_MSG_ID_WAYPOINT_ACK:
{
//send_text(SEVERITY_LOW,"waypoint ack");
// decode
mavlink_waypoint_ack_t packet;
mavlink_msg_waypoint_ack_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// check for error
uint8_t type = packet.type; // ok (0), error(1)
// turn off waypoint send
global_data.waypoint_sending = false;
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
//send_text(SEVERITY_LOW,"param request list");
// decode
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// Start sending parameters
global_data.parameter_i = 0;
}
break;
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{
//send_text(SEVERITY_LOW,"waypoint clear all");
// decode
mavlink_waypoint_clear_all_t packet;
mavlink_msg_waypoint_clear_all_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// clear all waypoints
uint8_t type = 0; // ok (0), error(1)
set(PARAM_WP_TOTAL,0);
// send acknowledgement 3 times to makes sure it is received
for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
//send_text(SEVERITY_LOW,"waypoint set current");
// decode
mavlink_waypoint_set_current_t packet;
mavlink_msg_waypoint_set_current_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// set current waypoint
set(PARAM_WP_INDEX,packet.seq);
{
Location temp; // XXX this is gross
temp = get_wp_with_index(packet.seq);
set_next_WP(&temp);
}
mavlink_msg_waypoint_current_send(chan,get(PARAM_WP_INDEX));
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
//send_text(SEVERITY_LOW,"waypoint count");
// decode
mavlink_waypoint_count_t packet;
mavlink_msg_waypoint_count_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// start waypoint receiving
set(PARAM_WP_TOTAL,packet.count);
if (get(PARAM_WP_TOTAL) > MAX_WAYPOINTS)
set(PARAM_WP_TOTAL,MAX_WAYPOINTS);
global_data.waypoint_timelast_receive = millis();
global_data.waypoint_receiving = true;
global_data.waypoint_sending = false;
global_data.waypoint_request_i = 0;
break;
}
case MAVLINK_MSG_ID_WAYPOINT:
{
// Check if receiving waypiont
if (!global_data.waypoint_receiving) break;
// decode
mavlink_waypoint_t packet;
mavlink_msg_waypoint_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// check if this is the requested waypoint
if (packet.seq != global_data.waypoint_request_i) break;
// store waypoint
uint8_t loadAction = 0; // 0 insert in list, 1 exec now
switch (packet.frame)
{
case MAV_FRAME_GLOBAL:
{
tell_command.lng = 1.0e7f*packet.x;
tell_command.lat = 1.0e7f*packet.y;
tell_command.alt = packet.z*1.0e2f;
break;
}
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lng = 1.0e7f*ToDeg(packet.x/
(radius_of_earth*cosf(ToRad(home.lat/1.0e7f)))) + home.lng;
tell_command.lat = 1.0e7f*ToDeg(packet.y/radius_of_earth) + home.lat;
tell_command.alt = -packet.z*1.0e2f + home.alt;
break;
}
}
// defaults
tell_command.id = CMD_BLANK;
switch (packet.action)
{
case MAV_ACTION_TAKEOFF:
{
tell_command.id = CMD_TAKEOFF;
break;
}
case MAV_ACTION_LAND:
{
tell_command.id = CMD_LAND;
break;
}
case MAV_ACTION_NAVIGATE:
{
tell_command.id = CMD_WAYPOINT;
break;
}
case MAV_ACTION_LOITER:
{
tell_command.id = CMD_LOITER_TIME;
tell_command.p1 = packet.param2/1.0e2;
break;
}
}
// save waypoint
set_wp_with_index(tell_command, packet.seq);
// update waypoint receiving state machine
global_data.waypoint_timelast_receive = millis();
global_data.waypoint_request_i++;
if (global_data.waypoint_request_i == get(PARAM_WP_TOTAL))
{
gcs.send_text(SEVERITY_LOW,"flight plane received");
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
global_data.waypoint_receiving = false;
set(PARAM_WP_RADIUS,packet.param1); // XXX takes last waypoint radius for all
//save_EEPROM_waypoint_info();
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
// 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
const char * key = (const char*) packet.param_id;
// iterate known parameters
// XXX linear search; would be better to sort params & use a binary search
for (uint16_t i = 0; i < global_data.param_count; i++) {
// compare key with parameter name
if (!strcmp_P(key, getParamName(i))) {
// sanity-check the new parameter
if (!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
setParamAsFloat(i,packet.param_value);
// Report back new value
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
strcpy_P(param_name, getParamName(i)); /// XXX HACK
mavlink_msg_param_value_send(chan,
(int8_t*)param_name,
getParamAsFloat(i),
global_data.param_count,i);
// call load if required
if (i >= PARAM_RLL2SRV_P && i <= PARAM_RLL2SRV_IMAX) pidServoRoll.load_gains();
if (i >= PARAM_PTCH2SRV_P && i <= PARAM_PTCH2SRV_IMAX) pidServoPitch.load_gains();
if (i >= PARAM_YW2SRV_P && i <= PARAM_YW2SRV_IMAX) pidServoRudder.load_gains();
if (i >= PARAM_HDNG2RLL_P && i <= PARAM_HDNG2RLL_IMAX) pidNavRoll.load_gains();
if (i >= PARAM_ARSPD2PTCH_P && i <= PARAM_ARSPD2PTCH_IMAX) pidNavPitchAirspeed.load_gains();
if (i >= PARAM_ALT2PTCH_P && i <= PARAM_ALT2PTCH_IMAX) pidNavPitchAltitude.load_gains();
if (i >= PARAM_ENRGY2THR_P && i <= PARAM_ENRGY2THR_IMAX) pidTeThrottle.load_gains();
if (i >= PARAM_ALT2THR_P && i <= PARAM_ALT2THR_IMAX) pidAltitudeThrottle.load_gains();
}
break;
}
}
break;
} // end case
} // end switch
} // end handle mavlink
#endif

276
Tools/ArduTracker/GCS_Standard.pde
View File

@ -1,276 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
///
/// @file GCS_Standard.pde
/// @brief GCS driver for the APM binary protocol
///
#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD
// constructor
GCS_STANDARD::GCS_STANDARD(BinComm::MessageHandler GCS_MessageHandlers[]) :
_binComm(GCS_MessageHandlers)
{
}
void
GCS_STANDARD::init(BetterStream *port)
{
GCS_Class::init(port);
_binComm.init(port);
}
void
GCS_STANDARD::update()
{
_binComm.update();
}
void
GCS_STANDARD::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
_binComm.send_msg_acknowledge(id, sum1, sum2);
gcs_messages_sent++;
}
void
GCS_STANDARD::send_message(uint8_t id, uint32_t param)
{
byte mess_buffer[54];
byte mess_ck_a = 0;
byte mess_ck_b = 0;
int tempint;
int ck;
long templong;
switch(id) {
case MSG_MODE_CHANGE:
case MSG_HEARTBEAT:
_binComm.send_msg_heartbeat(control_mode, // current control mode
millis() / 1000, // seconds since power-up
battery_voltage1 * 100, // battery voltage * 100
command_must_index); // command index (waypoint #)
break;
case MSG_ATTITUDE:
_binComm.send_msg_attitude(roll_sensor,
pitch_sensor,
yaw_sensor);
break;
case MSG_LOCATION:
_binComm.send_msg_location(current_loc.lat,
current_loc.lng,
GPS.altitude / 100,
GPS.ground_speed,
yaw_sensor,
GPS.time);
break;
case MSG_PRESSURE:
_binComm.send_msg_pressure(current_loc.alt / 10,
airspeed / 100);
break;
case MSG_PERF_REPORT:
_binComm.send_msg_perf_report(millis() - perf_mon_timer,
mainLoop_count,
G_Dt_max & 0xff,
gyro_sat_count,
adc_constraints,
renorm_sqrt_count,
renorm_blowup_count,
gps_fix_count,
imu_health * 100,
gcs_messages_sent);
break;
case MSG_CPU_LOAD:
//TODO: Implement appropriate BinComm routine here
case MSG_VALUE:
switch(param) {
//case 0x00: templong = roll_mode; break;
//case 0x01: templong = pitch_mode; break;
//case 0x02: templong = throttle_mode; break;
case 0x03: templong = yaw_mode; break;
case 0x04: templong = elevon1_trim; break;
case 0x05: templong = elevon2_trim; break;
/* TODO: implement for new PID lib
case 0x10: templong = integrator[0] * 1000; break;
case 0x11: templong = integrator[1] * 1000; break;
case 0x12: templong = integrator[2] * 1000; break;
case 0x13: templong = integrator[3] * 1000; break;
case 0x14: templong = integrator[4] * 1000; break;
case 0x15: templong = integrator[5] * 1000; break;
case 0x16: templong = integrator[6] * 1000; break;
case 0x17: templong = integrator[7] * 1000; break;
*/
case 0x1a: templong = kff[0]; break;
case 0x1b: templong = kff[1]; break;
case 0x1c: templong = kff[2]; break;
case 0x20: templong = target_bearing; break;
case 0x21: templong = nav_bearing; break;
case 0x22: templong = bearing_error; break;
case 0x23: templong = crosstrack_bearing; break;
case 0x24: templong = crosstrack_error; break;
case 0x25: templong = altitude_error; break;
case 0x26: templong = wp_radius; break;
case 0x27: templong = loiter_radius; break;
// case 0x28: templong = wp_mode; break;
// case 0x29: templong = loop_commands; break;
case 0x2a: templong = nav_gain_scaler; break;
}
_binComm.send_msg_value(param,
templong);
break;
case MSG_COMMAND_LIST:
tell_command = get_wp_with_index((int)param);
_binComm.send_msg_command_list(param,
wp_total,
tell_command.id,
tell_command.p1,
tell_command.alt,
tell_command.lat,
tell_command.lng);
break;
case MSG_TRIM_STARTUP:
_binComm.send_msg_trim_startup(radio_trim);
break;
case MSG_TRIM_MIN:
_binComm.send_msg_trim_min(radio_min);
break;
case MSG_TRIM_MAX:
_binComm.send_msg_trim_max(radio_max);
break;
/* TODO: implement for new PID lib
case MSG_PID: // PID Gains message
_binComm.send_msg_pid(param,
kp[param] * 1000000,
ki[param] * 1000000,
kd[param] * 1000000,
integrator_max[param]);
break;
*/
case MSG_SERVO_OUT:
_binComm.send_msg_servo_out(servo_out);
break;
case MSG_RADIO_OUT:
_binComm.send_msg_radio_out(radio_out);
break;
default:
GCS.send_text(SEVERITY_LOW,"<send_message> unknown message ID");
}
gcs_messages_sent++;
}
void
GCS_STANDARD::send_text(byte severity, const char *str)
{
if (severity >= DEBUG_LEVEL) {
char text[50]; // XXX magic numbers
strncpy(text, str, 50);
_binComm.send_msg_status_text(severity, text);
gcs_messages_sent++;
}
}
void
receive_message(void * arg, uint8_t id, uint8_t messageVersion, void * messageData)
{
// process command
switch(id) {
case MSG_STATUS_TEXT:
{
char str[50];
uint8_t severity;
GCS.getBinComm().unpack_msg_status_text(severity,str);
SendDebug(str);
SendDebug(" severity: "); SendDebugln(severity);
}
break;
case MSG_VERSION_REQUEST:
break;
case MSG_VALUE_REQUEST:
break;
case MSG_VALUE_SET:
break;
case MSG_PID_REQUEST:
break;
case MSG_PID_SET:
break;
case MSG_EEPROM_REQUEST:
break;
case MSG_EEPROM_SET:
break;
case MSG_PIN_REQUEST:
break;
case MSG_PIN_SET:
break;
case MSG_DATAFLASH_REQUEST:
break;
case MSG_DATAFLASH_SET:
break;
case MSG_COMMAND_REQUEST:
uint16_t commandIndex;
GCS.getBinComm().unpack_msg_command_request(commandIndex);
tell_command = get_wp_with_index(commandIndex);
GCS.getBinComm().send_msg_command_list(commandIndex,uint16_t(wp_total),tell_command.id,
tell_command.p1,tell_command.alt,tell_command.lat,tell_command.lng);
break;
case MSG_COMMAND_UPLOAD:
uint8_t action; // 0 -insert in list, 1- execute immediately
uint16_t itemNumber; // item number ( i.e. waypoint number)
uint16_t listLength; // list length
struct Location temp;
GCS.getBinComm().unpack_msg_command_upload(action,itemNumber,listLength,temp.id,temp.p1,temp.alt,temp.lat,temp.lng);
wp_total=listLength;
if (action == 0) // insert in list
{
// save waypoint total to eeprom at start of the list
if (itemNumber == 1) save_EEPROM_waypoint_info();
set_wp_with_index(temp, itemNumber);
}
else if (action == 1)
{
next_command = temp;
process_now();
}
break;
case MSG_ACKNOWLEDGE:
break;
default:
{
char str[50];
sprintf(str,"<receive_message> unknown messageID:%x",id);
GCS.send_text(SEVERITY_LOW,str);
}
}
}
#endif // GCS_PROTOCOL_STANDARD

113
Tools/ArduTracker/GCS_Xplane.pde
View File

@ -1,113 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
///
/// @file GCS_Xplane.pde
/// @brief GCS driver for the X-plane HIL interface.
///
#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE
void
GCS_XPLANE::send_text(uint8_t severity, const char *str)
{
if(severity >= DEBUG_LEVEL){
Serial.print("MSG: ");
Serial.println(str);
}
}
void
GCS_XPLANE::send_message(uint8_t id, uint32_t param)
{
switch(id) {
case MSG_HEARTBEAT:
print_control_mode();
break;
case MSG_ATTITUDE:
//print_attitude();
break;
case MSG_LOCATION:
//print_position();
break;
case MSG_CPU_LOAD:
//TODO: implement appropriate routine here if applicable
break;
case MSG_COMMAND_LIST:
struct Location cmd = get_wp_with_index(param);
print_waypoint(&cmd, param);
break;
}
}
void
GCS_XPLANE::print_control_mode(void)
{
Serial.print("MSG: ");
Serial.print(flight_mode_strings[control_mode]);
}
void
GCS_XPLANE::print_current_waypoints()
{
Serial.print("MSG: ");
Serial.print("CUR:");
Serial.print("\t");
Serial.print(current_loc.lat,DEC);
Serial.print(",\t");
Serial.print(current_loc.lng,DEC);
Serial.print(",\t");
Serial.println(current_loc.alt,DEC);
Serial.print("MSG: ");
Serial.print("NWP:");
Serial.print(wp_index,DEC);
Serial.print(",\t");
Serial.print(next_WP.lat,DEC);
Serial.print(",\t");
Serial.print(next_WP.lng,DEC);
Serial.print(",\t");
Serial.println(next_WP.alt,DEC);
}
void
GCS_XPLANE::print_waypoint(struct Location *cmd,byte index)
{
Serial.print("MSG: command #: ");
Serial.print(index, DEC);
Serial.print(" id: ");
Serial.print(cmd->id,DEC);
Serial.print(" p1: ");
Serial.print(cmd->p1,DEC);
Serial.print(" p2: ");
Serial.print(cmd->alt,DEC);
Serial.print(" p3: ");
Serial.print(cmd->lat,DEC);
Serial.print(" p4: ");
Serial.println(cmd->lng,DEC);
}
void
GCS_XPLANE::print_waypoints()
{
Serial.println("Commands in memory");
Serial.print("commands total: ");
Serial.println(wp_total, DEC);
// create a location struct to hold the temp Waypoints for printing
//Location tmp;
Serial.println("Home: ");
struct Location cmd = get_wp_with_index(0);
print_waypoint(&cmd, 0);
Serial.println("Commands: ");
for (int i=1; i<wp_total; i++){
cmd = get_wp_with_index(i);
print_waypoint(&cmd, i);
}
}
#endif // GCS_PROTOCOL_XPLANE

141
Tools/ArduTracker/HIL.h
View File

@ -1,141 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file HIL.h
/// @brief Interface definition for the various Ground Control System protocols.
#ifndef __HIL_H
#define __HIL_H
# if HIL_MODE != HIL_MODE_DISABLED
#include <BetterStream.h>
#include <AP_Common.h>
#include <APM_BinComm.h>
#include <GPS.h>
#include <Stream.h>
#include <stdint.h>
///
/// @class HIL
/// @brief Class describing the interface between the APM code
/// proper and the HIL implementation.
///
/// HIL' are currently implemented inside the sketch and as such have
/// access to all global state. The sketch should not, however, call HIL
/// internal functions - all calls to the HIL should be routed through
/// this interface (or functions explicitly exposed by a subclass).
///
class HIL_Class
{
public:
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required before
/// HIL 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(BetterStream *port) { _port = port; }
/// Update HIL 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 HIL 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(uint8_t id, int32_t param = 0) {}
/// Send a text message.
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const char *str) {}
/// Send acknowledgement for a message.
///
/// @param id The message ID being acknowledged.
/// @param sum1 Checksum byte 1 from the message being acked.
/// @param sum2 Checksum byte 2 from the message being acked.
///
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {}
protected:
/// The stream we are communicating over
BetterStream *_port;
};
//
// HIL class definitions.
//
// These are here so that we can declare the HIL object early in the sketch
// and then reference it statically rather than via a pointer.
//
///
/// @class HIL_MAVLINK
/// @brief The mavlink protocol for hil
///
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
class HIL_MAVLINK : public HIL_Class
{
public:
HIL_MAVLINK();
void update(void);
void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
private:
void handleMessage(mavlink_message_t * msg);
mavlink_channel_t chan;
uint16_t packet_drops;
};
#endif // HIL_PROTOCOL_MAVLINK
///
/// @class HIL_XPLANE
/// @brief The xplane protocol for hil
///
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
class HIL_XPLANE : public HIL_Class
{
public:
HIL_XPLANE();
void update(void);
void init(BetterStream *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
private:
void output_HIL();
void output_HIL_();
void output_int(int value);
void output_byte(byte value);
void print_buffer();
AP_GPS_IMU * hilPacketDecoder;
byte buf_len;
byte out_buffer[32];
};
#endif // HIL_PROTOCOL_XPLANE
#endif // HIL not disabled
#endif // __HIL_H

154
Tools/ArduTracker/HIL_Mavlink.pde
View File

@ -1,154 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
#include "Mavlink_Common.h"
HIL_MAVLINK::HIL_MAVLINK() :
packet_drops(0)
{
}
void
HIL_MAVLINK::init(BetterStream * port)
{
HIL_Class::init(port);
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
}
void
HIL_MAVLINK::update(void)
{
mavlink_message_t msg;
mavlink_status_t status;
// process received bytes
while(comm_get_available(chan))
{
uint8_t c = comm_receive_ch(chan);
// Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
}
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
}
void
HIL_MAVLINK::send_message(uint8_t id, uint32_t param)
{
mavlink_send_message(chan,id,param,packet_drops);
}
void
HIL_MAVLINK::send_text(uint8_t severity, const char *str)
{
mavlink_send_text(chan,severity,str);
}
void
HIL_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
mavlink_acknowledge(chan,id,sum1,sum2);
}
void
HIL_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// handle incoming vehicle position
case MAVLINK_MSG_ID_GLOBAL_POSITION:
{
// decode
mavlink_global_position_t packet;
mavlink_msg_global_position_decode(msg, &packet);
//if (mavlink_check_target(packet.target_system,packet.target_component)) break;
trackVehicle_loc.id = 0;
trackVehicle_loc.p1 = 0;
trackVehicle_loc.alt = packet.alt;
trackVehicle_loc.lat = packet.lat;
trackVehicle_loc.lng = packet.lon;
Serial.println("received global position packet");
}
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode.
case MAVLINK_MSG_ID_GPS_RAW:
{
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
// set gps hil sensor
gps.setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
packet.v,packet.hdg,0,0);
break;
}
case MAVLINK_MSG_ID_AIRSPEED:
{
// decode
mavlink_airspeed_t packet;
mavlink_msg_airspeed_decode(msg, &packet);
// set airspeed
airspeed = 100*packet.airspeed;
break;
}
#if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE:
{
// decode
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set gps hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
break;
}
#elif HIL_MODE == HIL_MODE_SENSORS
case MAVLINK_MSG_ID_RAW_IMU:
{
// decode
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
// set imu hil sensors
// TODO: check scaling for temp/absPress
float temp = 70;
float absPress = 1;
//Serial.printf_P(PSTR("\nreceived accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
//Serial.printf_P(PSTR("\nreceived gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
adc.setHIL(packet.xgyro,packet.ygyro,packet.zgyro,temp,
packet.xacc,packet.yacc,packet.zacc,absPress);
compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
break;
}
case MAVLINK_MSG_ID_RAW_PRESSURE:
{
// decode
mavlink_raw_pressure_t packet;
mavlink_msg_raw_pressure_decode(msg, &packet);
// set pressure hil sensor
// TODO: check scaling
float temp = 70;
pitot.setHIL(temp,packet.press_diff1);
break;
}
#endif // HIL_MODE
} // end switch
}
#endif

151
Tools/ArduTracker/HIL_Xplane.pde
View File

@ -1,151 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
void HIL_XPLANE::output_HIL(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(servo_out[CH_ROLL])); // 0 bytes 0, 1
output_int((int)(servo_out[CH_PITCH])); // 1 bytes 2, 3
output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4, 5
output_int((int)(servo_out[CH_RUDDER])); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)next_WP.alt / 100); // 6 bytes 12, 13
output_int((int)energy_error); // 7 bytes 14,15
output_byte(get(PARAM_WP_INDEX)); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17
// print out the buffer and checksum
// ---------------------------------
print_buffer();
}
// This is for debugging only!,
// I just move the underscore to keep the above version pristene.
void HIL_XPLANE::output_HIL_(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(servo_out[CH_ROLL])); // 0 bytes 0, 1
output_int((int)(servo_out[CH_PITCH])); // 1 bytes 2, 3
output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4, 5
output_int((int)(servo_out[CH_RUDDER])); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8, 9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)dcm.roll_sensor); // 6 bytes 12,13
output_int((int)loiter_total); // 7 bytes 14,15
output_byte(get(PARAM_WP_INDEX)); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17
// print out the buffer and checksum
// ---------------------------------
print_buffer();
}
void HIL_XPLANE::output_int(int value)
{
//buf_len += 2;
out_buffer[buf_len++] = value & 0xff;
out_buffer[buf_len++] = (value >> 8) & 0xff;
}
void HIL_XPLANE::output_byte(byte value)
{
out_buffer[buf_len++] = value;
}
void HIL_XPLANE::print_buffer()
{
byte ck_a = 0;
byte ck_b = 0;
for (byte i = 0;i < buf_len; i++){
Serial.print (out_buffer[i]);
}
Serial.print('\r');
Serial.print('\n');
buf_len = 0;
}
HIL_XPLANE::HIL_XPLANE() :
buf_len(0)
{
}
void
HIL_XPLANE::init(BetterStream * port)
{
HIL_Class::init(port);
hilPacketDecoder = new AP_GPS_IMU(port);
hilPacketDecoder->init();
}
void
HIL_XPLANE::update(void)
{
hilPacketDecoder->update();
airspeed = (float)hilPacketDecoder->airspeed; //airspeed is * 100 coming in from Xplane for accuracy
calc_airspeed_errors();
dcm.setHil(hilPacketDecoder->roll_sensor*M_PI/18000,
hilPacketDecoder->pitch_sensor*M_PI/18000,
hilPacketDecoder->ground_course*M_PI/18000,
0,0,0);
// set gps hil sensor
gps.setHIL(hilPacketDecoder->time/1000.0,(float)hilPacketDecoder->latitude/1.0e7,(float)hilPacketDecoder->longitude/1.0e7,(float)hilPacketDecoder->altitude/1.0e2,
(float)hilPacketDecoder->speed_3d/1.0e2,(float)hilPacketDecoder->ground_course/1.0e2,0,0);
}
void
HIL_XPLANE::send_message(uint8_t id, uint32_t param)
{
// TODO: split output by actual request
uint64_t timeStamp = micros();
switch(id) {
case MSG_HEARTBEAT:
break;
case MSG_EXTENDED_STATUS:
break;
case MSG_ATTITUDE:
break;
case MSG_LOCATION:
break;
case MSG_LOCAL_LOCATION:
break;
case MSG_GPS_RAW:
break;
case MSG_SERVO_OUT:
output_HIL();
break;
case MSG_AIRSPEED:
break;
case MSG_RADIO_OUT:
break;
case MSG_RAW_IMU:
break;
case MSG_GPS_STATUS:
break;
case MSG_CURRENT_WAYPOINT:
break;
defualt:
break;
}
}
void
HIL_XPLANE::send_text(uint8_t severity, const char *str)
{
}
void
HIL_XPLANE::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
}
#endif

611
Tools/ArduTracker/Log.pde
View File

@ -1,611 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs
#define HEAD_BYTE1 0xA3 // Decimal 163
#define HEAD_BYTE2 0x95 // Decimal 149
#define END_BYTE 0xBA // Decimal 186
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv);
static int8_t dump_log(uint8_t argc, const Menu::arg *argv);
static int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads from flash memory
static int8_t help_log(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\n"
"Commands:\n"
" dump <n> dump log <n>\n"
" erase erase all logs\n"
" enable <name>|all enable logging <name> or everything\n"
" disable <name>|all disable logging <name> or everything\n"
"\n"));
}
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
{"disable", select_logs},
{"help", help_log}
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
static bool
print_log_menu(void)
{
int log_start;
int log_end;
byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM);
Serial.printf_P(PSTR("logs enabled: "));
if (0 == get(PARAM_LOG_BITMASK)) {
Serial.printf_P(PSTR("none"));
} else {
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (get(PARAM_LOG_BITMASK) & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED);
PLOG(GPS);
PLOG(PM);
PLOG(CTUN);
PLOG(NTUN);
PLOG(MODE);
PLOG(RAW);
PLOG(CMD);
#undef PLOG
}
Serial.println();
if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs available for download\n"));
} else {
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
for(int i=1;i<last_log_num+1;i++) {
log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i-1)*0x02));
log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i)*0x02))-1;
if (i == last_log_num) {
log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE);
}
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"),
i, log_start, log_end);
}
Serial.println();
}
return(true);
}
static int8_t
dump_log(uint8_t argc, const Menu::arg *argv)
{
byte dump_log;
int dump_log_start;
int dump_log_end;
byte last_log_num;
// check that the requested log number can be read
dump_log = argv[1].i;
last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM);
if ((argc != 2) || (dump_log < 1) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log number\n"));
return(-1);
}
dump_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log-1)*0x02));
dump_log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log)*0x02))-1;
if (dump_log == last_log_num) {
dump_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE);
}
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
dump_log, dump_log_start, dump_log_end);
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Log read complete\n"));
}
static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv)
{
for(int i = 10 ; i > 0; i--) {
Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i);
delay(1000);
}
Serial.printf_P(PSTR("\nErasing log...\n"));
for(int j = 1; j < 4001; j++)
DataFlash.PageErase(j);
eeprom_write_byte((uint8_t *)EE_LAST_LOG_NUM, 0);
eeprom_write_byte((uint8_t *)EE_LAST_LOG_PAGE, 1);
Serial.printf_P(PSTR("\nLog erased.\n"));
}
static int8_t
select_logs(uint8_t argc, const Menu::arg *argv)
{
uint16_t bits;
if (argc != 2) {
Serial.printf_P(PSTR("missing log type\n"));
return(-1);
}
bits = 0;
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// that name as the argument to the command, and set the bit in
// bits accordingly.
//
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~(bits = 0);
} else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s
TARG(ATTITUDE_FAST);
TARG(ATTITUDE_MED);
TARG(GPS);
TARG(PM);
TARG(CTUN);
TARG(NTUN);
TARG(MODE);
TARG(RAW);
TARG(CMD);
#undef TARG
}
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
set(PARAM_LOG_BITMASK, get(PARAM_LOG_BITMASK) | bits);
} else {
set(PARAM_LOG_BITMASK, get(PARAM_LOG_BITMASK) & ~bits);
}
return(0);
}
int8_t
process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
}
// Write an attitude packet. Total length : 10 bytes
void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(log_roll);
DataFlash.WriteInt(log_pitch);
DataFlash.WriteInt(log_yaw);
DataFlash.WriteByte(END_BYTE);
}
// Write a performance monitoring packet. Total length : 19 bytes
void Log_Write_Performance()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteLong(millis()- perf_mon_timer);
DataFlash.WriteInt(mainLoop_count);
DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(gyro_sat_count);
DataFlash.WriteByte(adc_constraints);
DataFlash.WriteByte(renorm_sqrt_count);
DataFlash.WriteByte(renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(imu_health*1000));
DataFlash.WriteByte(END_BYTE);
}
// Write a command processing packet. Total length : 19 bytes
//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
void Log_Write_Cmd(byte num, struct Location *wp)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
}
void Log_Write_Startup(byte type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type);
DataFlash.WriteByte(get(PARAM_WP_TOTAL));
DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_wp_with_index(0);
Log_Write_Cmd(0, &cmd);
for (int i=1; i<get(PARAM_WP_TOTAL); i++){
cmd = get_wp_with_index(i);
Log_Write_Cmd(i, &cmd);
}
}
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Control_Tuning()
{
Vector3f accel = imu.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(servo_out[CH_ROLL]));
DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)(servo_out[CH_PITCH]));
DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((int)(servo_out[CH_THROTTLE]));
DataFlash.WriteInt((int)(servo_out[CH_RUDDER]));
DataFlash.WriteInt((int)(accel.y*10000));
DataFlash.WriteByte(END_BYTE);
}
#endif
// Write a navigation tuning packet. Total length : 18 bytes
void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)wp_distance);
DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing);
DataFlash.WriteInt(altitude_error);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteInt((int)(nav_gain_scaler*1000));
DataFlash.WriteByte(END_BYTE);
}
// Write a mode packet. Total length : 5 bytes
void Log_Write_Mode(byte mode)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_MODE_MSG);
DataFlash.WriteByte(mode);
DataFlash.WriteByte(END_BYTE);
}
// Write an GPS packet. Total length : 30 bytes
void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_mix_alt, long log_gps_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(log_Time);
DataFlash.WriteByte(log_Fix);
DataFlash.WriteByte(log_NumSats);
DataFlash.WriteLong(log_Lattitude);
DataFlash.WriteLong(log_Longitude);
DataFlash.WriteLong(log_mix_alt);
DataFlash.WriteLong(log_gps_alt);
DataFlash.WriteLong(log_Ground_Speed);
DataFlash.WriteLong(log_Ground_Course);
DataFlash.WriteByte(END_BYTE);
DataFlash.WriteByte(END_BYTE);
}
// Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Raw()
{
Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel();
gyro *= t7; // Scale up for storage as long integers
accel *= t7;
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
DataFlash.WriteLong((long)gyro.x);
DataFlash.WriteLong((long)gyro.y);
DataFlash.WriteLong((long)gyro.z);
DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z);
DataFlash.WriteByte(END_BYTE);
}
#endif
// Read an control tuning packet
void Log_Read_Control_Tuning()
{
float logvar;
Serial.print("CTUN:");
for (int y=1;y<10;y++) {
logvar = DataFlash.ReadInt();
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read a nav tuning packet
void Log_Read_Nav_Tuning()
{
Serial.print("NTUN:");
Serial.print((float)((uint16_t)DataFlash.ReadInt())/100.0); // Yaw from IMU
Serial.print(comma);
Serial.print(DataFlash.ReadInt()); // wp_distance
Serial.print(comma);
Serial.print((float)((uint16_t)DataFlash.ReadInt())/100.0); // target_bearing - Bearing to Target
Serial.print(comma);
Serial.print((float)((uint16_t)DataFlash.ReadInt())/100.0); // nav_bearing - Bearing to steer
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/100.0); // Altitude error
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/100.0); // Airspeed
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/1000.0); // nav_gain_scaler
Serial.println(comma);
}
// Read a performance packet
void Log_Read_Performance()
{
long pm_time;
int logvar;
Serial.print("PM:");
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
Serial.print(comma);
for (int y=1;y<9;y++) {
if(y<3 || y>7) logvar = DataFlash.ReadInt();
else logvar = DataFlash.ReadByte();
//if(y > 7) logvar = logvar/1000.f;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read a command processing packet
void Log_Read_Cmd()
{
byte logvarb;
long logvarl;
Serial.print("CMD:");
for(int i=1;i<4;i++) {
logvarb = DataFlash.ReadByte();
Serial.print(logvarb,DEC);
Serial.print(comma);
}
for(int i=1;i<4;i++) {
logvarl = DataFlash.ReadLong();
Serial.print(logvarl,DEC);
Serial.print(comma);
}
Serial.println(" ");
}
void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
if (logbyte == TYPE_AIRSTART_MSG)
Serial.print("AIR START - ");
else if (logbyte == TYPE_GROUNDSTART_MSG)
Serial.print("GROUND START - ");
else
Serial.print("UNKNOWN STARTUP TYPE -");
Serial.print(DataFlash.ReadByte(), DEC);
Serial.println(" commands in memory");
}
// Read an attitude packet
void Log_Read_Attitude()
{
int log_roll;
int log_pitch;
uint16_t log_yaw;
log_roll = DataFlash.ReadInt();
log_pitch = DataFlash.ReadInt();
log_yaw = (uint16_t)DataFlash.ReadInt();
Serial.print("ATT:");
Serial.print(log_roll);
Serial.print(comma);
Serial.print(log_pitch);
Serial.print(comma);
Serial.print(log_yaw);
Serial.println();
}
// Read a mode packet
void Log_Read_Mode()
{
byte mode;
mode = DataFlash.ReadByte();
Serial.print("MOD:");
switch (mode) {
case 0:
Serial.println("Manual");
break;
case 1:
Serial.println("Stab");
break;
case 5:
Serial.println("FBW_A");
break;
case 6:
Serial.println("FBW_B");
break;
case 10:
Serial.println("AUTO");
break;
case 11:
Serial.println("RTL");
break;
case 12:
Serial.println("Loiter");
break;
case 98:
Serial.println("AS_COM");
break;
case 99:
Serial.println("GS_COM");
break;
}
}
// Read a GPS packet
void Log_Read_GPS()
{
Serial.print("GPS:");
Serial.print(DataFlash.ReadLong()); // Time
Serial.print(comma);
Serial.print((int)DataFlash.ReadByte()); // Fix
Serial.print(comma);
Serial.print((int)DataFlash.ReadByte()); // Num of Sats
Serial.print(comma);
Serial.print((float)DataFlash.ReadLong()/t7, 7); // Lattitude
Serial.print(comma);
Serial.print((float)DataFlash.ReadLong()/t7, 7); // Longitude
Serial.print(comma);
Serial.print((float)DataFlash.ReadLong()/100.0); // Baro/gps altitude mix
Serial.print(comma);
Serial.print((float)DataFlash.ReadLong()/100.0); // GPS altitude
Serial.print(comma);
Serial.print((float)DataFlash.ReadLong()/100.0); // Ground Speed
Serial.print(comma);
Serial.println((float)DataFlash.ReadLong()/100.0); // Ground Course
}
// Read a raw accel/gyro packet
void Log_Read_Raw()
{
float logvar;
Serial.print("RAW:");
for (int y=0;y<6;y++) {
logvar = (float)DataFlash.ReadLong()/t7;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read the DataFlash log memory : Packet Parser
void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step=0;
int packet_count=0;
int page = start_page;
DataFlash.StartRead(start_page);
while (page < end_page && page != -1)
{
data = DataFlash.ReadByte();
switch(log_step) //This is a state machine to read the packets
{
case 0:
if(data==HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data==HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data==LOG_ATTITUDE_MSG){
Log_Read_Attitude();
log_step++;
}else if(data==LOG_MODE_MSG){
Log_Read_Mode();
log_step++;
}else if(data==LOG_CONTROL_TUNING_MSG){
Log_Read_Control_Tuning();
log_step++;
}else if(data==LOG_NAV_TUNING_MSG){
Log_Read_Nav_Tuning();
log_step++;
}else if(data==LOG_PERFORMANCE_MSG){
Log_Read_Performance();
log_step++;
}else if(data==LOG_RAW_MSG){
Log_Read_Raw();
log_step++;
}else if(data==LOG_CMD_MSG){
Log_Read_Cmd();
log_step++;
}else if(data==LOG_STARTUP_MSG){
Log_Read_Startup();
log_step++;
}else {
if(data==LOG_GPS_MSG){
Log_Read_GPS();
log_step++;
} else {
Serial.print("Error Reading Packet: ");
Serial.print(packet_count);
log_step=0; // Restart, we have a problem...
}
}
break;
case 3:
if(data==END_BYTE){
packet_count++;
}else{
Serial.print("Error Reading END_BYTE ");
Serial.println(data,DEC);
}
log_step=0; // Restart sequence: new packet...
break;
}
page = DataFlash.GetPage();
}
Serial.print("Number of packets read: ");
Serial.println(packet_count);
}

219
Tools/ArduTracker/Mavlink_Common.h
View File

@ -1,219 +0,0 @@
#ifndef Mavlink_Common_H
#define Mavlink_Common_H
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLIK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
uint16_t system_type = MAV_FIXED_WING;
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
{
if (sysid != mavlink_system.sysid)
{
return 1;
}
else if (compid != mavlink_system.compid)
{
gcs.send_text(SEVERITY_LOW,"component id mismatch");
return 0; // XXX currently not receiving correct compid from gcs
}
else return 0; // no error
}
/**
* @brief Send low-priority messages at a maximum rate of xx Hertz
*
* This function sends messages at a lower rate to not exceed the wireless
* bandwidth. It sends one message each time it is called until the buffer is empty.
* Call this function with xx Hertz to increase/decrease the bandwidth.
*/
static void mavlink_queued_send(mavlink_channel_t chan)
{
//send parameters one by one
if (global_data.parameter_i < global_data.param_count)
{
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
strcpy_P(param_name, getParamName(global_data.parameter_i)); /// XXX HACK
mavlink_msg_param_value_send(chan,
(int8_t*)param_name,
getParamAsFloat(global_data.parameter_i),
global_data.param_count,global_data.parameter_i);
global_data.parameter_i++;
}
// request waypoints one by one
if (global_data.waypoint_receiving &&
global_data.waypoint_request_i < get(PARAM_WP_TOTAL))
{
mavlink_msg_waypoint_request_send(chan,
global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid ,global_data.waypoint_request_i);
}
}
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
{
uint64_t timeStamp = micros();
switch(id) {
case MSG_HEARTBEAT:
mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
case MSG_EXTENDED_STATUS:
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case CIRCLE:
mode = MAV_MODE_TEST3;
break;
case STABILIZE:
mode = MAV_MODE_GUIDED;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case TAKEOFF:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LIFTOFF;
break;
case LAND:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LANDING;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint8_t motor_block = false;
mavlink_msg_sys_status_send(chan,mode,nav_mode,status,load*1000,
battery_voltage1*1000,motor_block,packet_drops);
break;
}
case MSG_ATTITUDE:
{
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(chan,timeStamp,dcm.roll,dcm.pitch,dcm.yaw,
omega.x,omega.y,omega.z);
break;
}
case MSG_LOCATION:
{
float gamma = dcm.pitch; // neglecting angle of attack for now
float yaw = dcm.yaw;
mavlink_msg_global_position_send(chan,timeStamp,current_loc.lat/1.0e7f,
current_loc.lng/1.0e7f,current_loc.alt/1.0e2f,gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw),
gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma));
break;
}
case MSG_LOCAL_LOCATION:
{
float gamma = dcm.pitch; // neglecting angle of attack for now
float yaw = dcm.yaw;
mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7f)*radius_of_earth,
ToRad((current_loc.lng-home.lng)/1.0e7f)*radius_of_earth*cosf(ToRad(home.lat/1.0e7f)),
(current_loc.alt-home.alt)/1.0e2f, gps.ground_speed/1.0e2f*cosf(gamma)*cosf(yaw),
gps.ground_speed/1.0e2f*cosf(gamma)*sinf(yaw),gps.ground_speed/1.0e2f*sinf(gamma));
break;
}
case MSG_GPS_RAW:
{
mavlink_msg_gps_raw_send(chan,timeStamp,gps.status(),
gps.latitude/1.0e7f,gps.longitude/1.0e7f,gps.altitude/100.0f,
2.0f,10.0f,gps.ground_speed/100.0f,gps.ground_course/100.0f);
break;
}
case MSG_AIRSPEED:
{
mavlink_msg_airspeed_send(chan,float(airspeed)/100.0);
break;
}
case MSG_SERVO_OUT:
{
uint8_t rssi = 1; // TODO: can we calculated this?
// receive signal strength 0(0%)-255(100%)
Serial.printf_P(PSTR("sending servo out: %d %d %d %d\n"),
servo_out[0],servo_out[1], servo_out[2], servo_out[3]);
mavlink_msg_rc_channels_scaled_send(chan,
servo_out[0],servo_out[1],
servo_out[2]*100, // account for throttle scaling 0-100
servo_out[3],servo_out[4],servo_out[5],
servo_out[6],servo_out[7],rssi);
break;
}
case MSG_RADIO_OUT:
{
uint8_t rssi = 1; // TODO: can we calculated this?
// receive signal strength 0(0%)-255(100%)
mavlink_msg_rc_channels_raw_send(chan,
radio_out[0],radio_out[1],radio_out[2],
radio_out[3],radio_out[4],radio_out[5],
radio_out[6],radio_out[7],rssi);
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU:
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(chan,timeStamp,
accel.x*1000.0f/gravity,accel.y*1000.0f/gravity,accel.z*1000.0f/gravity,
gyro.x*1000.0f,gyro.y*1000.0f,gyro.z*1000.0f,
compass.mag_x,compass.mag_y,compass.mag_z);
mavlink_msg_raw_pressure_send(chan,timeStamp,
adc.Ch(AIRSPEED_CH),pitot.RawPress,0);
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
mavlink_msg_gps_status_send(chan,gps.num_sats,NULL,NULL,NULL,NULL,NULL);
break;
}
case MSG_CURRENT_WAYPOINT:
{
mavlink_msg_waypoint_current_send(chan,get(PARAM_WP_INDEX));
break;
}
defualt:
break;
}
}
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
mavlink_msg_statustext_send(chan,severity,(const int8_t*)str);
}
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
{
}
#endif // mavlink in use
#endif // inclusion guard

118
Tools/ArduTracker/Terminal_commands.txt
View File

@ -1,118 +0,0 @@
<PID> = {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt}
show
heartbeat
attitude
location
command
severity <N>
hide
heartbeat
attitude
location
command
all
copy cmd <src> <target>
echo <text>
groundstart
inithome
k -?
print
altitude
altmaxcruiseerr
aspmaxcruiseerr
attitude
commands
ctrlmode
curwaypts
flightmodes
holdalt
imax <PID>
kp <PID>
ki <PID>
kd <PID>
kff {pitchcomp|ruddermix|pitchtothrottle}
kpitchcruise
location
navrolllimit
navsettings
pitchmax
pitchmin
pitchtarget
pressure
rlocation [home]
speed
targetaxis1
targetaxis2
targetneutral
throttlecruise
throttlemax
throttlemin
tuning
release
all
ctrlmode
rcin
reset commands
rtl
set
altmaxcruiseerr <X>
aspmaxcruiseerr <X>
cmd <commandindex> <commandtype> <param1> {here|{<alt> <lat> <lng>}}
commandtype = {waypoint|takeoff|land|landoptions|loiter|loiternturns|loitertime|delay|resetindex|airspeedcruise|throttlecruise|resethome|index|rtl|relay|servo}
cmdcount <N>
cmdindex <N>
cruise <X>
ctrlmode {manual|circle|stabilize|fbwa|fbwb|auto|rtl|loiter|takeoff|land|radio}
holdalt <X>
imax <PID>
kp <PID>
ki <PID>
kd <PID>
kff {pitchcomp|ruddermix|pitchtothrottle} <X>
kpitchcruise <X>
loiterradius <X>
navrolllimit <X>
pitchmax <X>
pitchmin <X>
pitchtarget <X>
rcin<N> <X>
rcout<N> <X>
target alt <X>
target here
targetaxis1 <X> <Y> <Z>
targetaxis2 <X> <Y> <Z>
targetmode <N>
targetneutral <X> <Y> <Z>
target <X> <Y> <Z>
throttlecruise <X>
throttlefailsafe <N>
throttlefailsafeaction <N>
throttlemax <X>
throttlemin <X>
wpradius <X>
xtrackentryangle <X>
xtrackgain <X>
status
climb
control
cruise
gps
landing
loiter
mixing
navigation
navpitch
navroll
pid <PID>
rc
rcinputch
rcin
rcout
rcpwm
rctrim
system
takeoff
telemetry
throttle
waypoints

92
Tools/ArduTracker/climb_rate.pde
View File

@ -1,92 +0,0 @@
struct DataPoint {
unsigned long x;
long y;
};
DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from
unsigned char index; // Index in history for the current data point
unsigned long xoffset;
unsigned char n;
// Intermediate variables for regression calculation
long xi;
long yi;
long xiyi;
unsigned long xi2;
void add_altitude_data(unsigned long xl, long y)
{
unsigned char i;
int dx;
//Reset the regression if our X variable overflowed
if (xl < xoffset)
n = 0;
//To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length
if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH)
n = 0;
if (n == ALTITUDE_HISTORY_LENGTH) {
xi -= history[index].x;
yi -= history[index].y;
xiyi -= (long)history[index].x * history[index].y;
xi2 -= history[index].x * history[index].x;
} else {
if (n == 0) {
xoffset = xl;
xi = 0;
yi = 0;
xiyi = 0;
xi2 = 0;
}
n++;
}
history[index].x = xl - xoffset;
history[index].y = y;
xi += history[index].x;
yi += history[index].y;
xiyi += (long)history[index].x * history[index].y;
xi2 += history[index].x * history[index].x;
if (++index >= ALTITUDE_HISTORY_LENGTH)
index = 0;
}
void recalc_climb_rate()
{
float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2);
climb_rate = (int)(slope*100);
}
void print_climb_debug_info()
{
unsigned char i, j;
recalc_climb_rate();
SendDebugln_P("Climb rate:");
for (i=0; i<ALTITUDE_HISTORY_LENGTH; i++) {
j = i + index;
if (j >= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH;
SendDebug_P(" ");
SendDebug(j,DEC);
SendDebug_P(": ");
SendDebug(history[j].x,DEC);
SendDebug_P(", ");
SendDebugln(history[j].y,DEC);
}
SendDebug_P(" sum(xi) = ");
SendDebugln(xi,DEC);
SendDebug_P(" sum(yi) = ");
SendDebugln(yi,DEC);
SendDebug_P(" sum(xi*yi) = ");
SendDebugln(xiyi,DEC);
SendDebug_P(" sum(xi^2) = ");
SendDebugln(xi2,DEC);
SendDebug_P(" Climb rate = ");
SendDebug((float)climb_rate/100,2);
SendDebugln_P(" m/s");
}

76
Tools/ArduTracker/command_description.txt
View File

@ -1,76 +0,0 @@
ArduPilotMega 1.0.0 Commands
Command Structure in bytes
0 0x00 byte Command ID
1 0x01 byte Parameter 1
2 0x02 int Parameter 2
3 0x03 ..
4 0x04 long Parameter 3
5 0x05 ..
6 0x06 ..
7 0x07 ..
8 0x08 long Parameter 4
9 0x09 ..
10 0x0A ..
11 0x0B ..
0x00 Reserved
....
0x0F Reserved
***********************************
Commands 0x10 to 0x2F are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
***********************************
Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 CMD_WAYPOINT - altitude lat lon
0x11 CMD_LOITER (indefinitely) - altitude lat lon
0x12 CMD_LOITER_N_TURNS turns altitude lat lon
0x13 CMD_LOITER_TIME time (seconds*10) altitude lat lon
0x14 CMD_RTL - altitude lat lon
0x15 CMD_LAND - altitude lat lon
0x16 CMD_TAKEOFF takeoff pitch altitude - -
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
***********************************
May Commands - these commands are optional to finish
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0x20 CMD_DELAY - - time (milliseconds) -
0x21 CMD_CLIMB rate (cm/sec) alt (finish) - -
0x22 CMD_LAND_OPTIONS distance to WP airspeed m/s, throttle %, pitch deg
***********************************
Unexecuted commands >= 0x20 are dropped when ready for the next command <= 0x1F so plan/queue commands accordingly!
For example if you had a string of 0x21 commands following a 0x10 command that had not finished when the waypoint was
reached, the unexecuted 0x21 commands would be skipped and the next command <= 0x1F would be loaded
***********************************
Now Commands - these commands are executed once until no more new now commands are available
0x31 CMD_RESET_INDEX
0x32 CMD_GOTO_INDEX index repeat count
0x33 CMD_GETVAR_INDEX variable ID
0x34 CMD_SENDVAR_INDEX off/on = 0/1
0x35 CMD_TELEMETRY off/on = 0/1
0x40 CMD_THROTTLE_CRUISE speed
0x41 CMD_AIRSPEED_CRUISE speed
0x44 CMD_RESET_HOME altitude lat lon
0x60 CMD_KP_GAIN array index gain value*100,000
0x61 CMD_KI_GAIN array index gain value*100,000
0x62 CMD_KD_GAIN array index gain value*100,000
0x63 CMD_KI_MAX array index gain value*100,000
0x64 CMD_KFF_GAIN array index gain value*100,000
0x70 CMD_RADIO_TRIM array index value
0x71 CMD_RADIO_MAX array index value
0x72 CMD_RADIO_MIN array index value
0x73 CMD_ELEVON_TRIM array index value (index 0 = elevon 1 trim, 1 = elevon 2 trim)
0x75 CMD_INDEX index
0x80 CMD_REPEAT type value delay in sec repeat count
0x81 CMD_RELAY (0,1 to change swicth position)
0x82 CMD_SERVO number value

239
Tools/ArduTracker/commands.pde
View File

@ -1,239 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
void init_commands()
{
//read_EEPROM_waypoint_info();
set(PARAM_WP_INDEX,0);
command_must_index = 0;
command_may_index = 0;
next_command.id = CMD_BLANK;
}
void update_auto()
{
if (get(PARAM_WP_INDEX) == get(PARAM_WP_TOTAL)){
return_to_launch();
//wp_index = 0;
}
}
void reload_commands()
{
init_commands();
//read_command_index(); // Get wp_index = command_must_index from EEPROM
if(get(PARAM_WP_INDEX) > 0){
decrement_WP_index;
}
}
// Getters
// -------
struct Location get_wp_with_index(int i)
{
struct Location temp;
long mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > get(PARAM_WP_TOTAL)) {
temp.id = CMD_BLANK;
}else{
// read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.p1 = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
}
return temp;
}
// Setters
// -------
void set_wp_with_index(struct Location temp, int i)
{
i = constrain(i,0,get(PARAM_WP_TOTAL));
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.p1);
mem++;
eeprom_write_dword((uint32_t *) mem, temp.alt);
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat);
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng);
}
void increment_WP_index()
{
if(get(PARAM_WP_INDEX) < get(PARAM_WP_TOTAL)){
set(PARAM_WP_INDEX, get(PARAM_WP_INDEX) +1);
SendDebug("MSG <increment_WP_index> WP index is incremented to ");
SendDebugln(get(PARAM_WP_INDEX),DEC);
}else{
SendDebug("MSG <increment_WP_index> Failed to increment WP index of ");
SendDebugln(get(PARAM_WP_INDEX),DEC);
}
}
void decrement_WP_index()
{
if(get(PARAM_WP_INDEX) > 0){
set(PARAM_WP_INDEX, get(PARAM_WP_INDEX) -1);
}
}
long read_alt_to_hold()
{
byte options = get(PARAM_CONFIG);
// save the alitude above home option
if(options & HOLD_ALT_ABOVE_HOME){
int32_t temp = get(PARAM_ALT_HOLD_HOME);
return temp + home.alt;
}else{
return current_loc.alt;
}
}
void loiter_at_location()
{
next_WP = current_loc;
}
// add a new command at end of command set to RTL.
void return_to_launch(void)
{
//so we know where we are navigating from
next_WP = current_loc;
// home is WP 0
// ------------
set(PARAM_WP_INDEX,0);
// Loads WP from Memory
// --------------------
set_next_WP(&home);
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
}
struct Location get_LOITER_home_wp()
{
// read home position
struct Location temp = get_wp_with_index(0);
temp.id = CMD_LOITER;
temp.alt = read_alt_to_hold();
return temp;
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
void set_next_WP(struct Location *wp)
{
//GCS.send_text(SEVERITY_LOW,"load WP");
SendDebug("MSG <set_next_wp> wp_index: ");
SendDebugln(get(PARAM_WP_INDEX),DEC);
gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX));
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP = next_WP;
// Load the next_WP slot
// ---------------------
next_WP = *wp;
// offset the altitude relative to home position
// ---------------------------------------------
next_WP.alt += home.alt;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
if(prev_WP.id != CMD_TAKEOFF && (next_WP.id == CMD_WAYPOINT || next_WP.id == CMD_LAND))
offset_altitude = next_WP.alt - prev_WP.alt;
else
offset_altitude = 0;
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
loiter_sum = 0;
loiter_total = 0;
float rads = (abs(next_WP.lat)/t7) * 0.0174532925f;
//377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943
scaleLongDown = cosf(rads);
scaleLongUp = 1.0f/cosf(rads);
// this is handy for the groundstation
wp_totalDistance = getDistance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
gcs.print_current_waypoints();
target_bearing = get_bearing(&current_loc, &next_WP);
old_target_bearing = target_bearing;
// this is used to offset the shrinking longitude as we go towards the poles
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
// run this at setup on the ground
// -------------------------------
void init_home()
{
SendDebugln("MSG: <init_home> init home");
// Extra read just in case
// -----------------------
//gps.Read();
// block until we get a good fix
// -----------------------------
while (!gps.new_data || !gps.fix) {
gps.update();
}
home.id = CMD_WAYPOINT;
home.lng = gps.longitude; // Lon * 10**7
home.lat = gps.latitude; // Lat * 10**7
home.alt = gps.altitude;
home_is_set = TRUE;
// ground altitude in centimeters for pressure alt calculations
// ------------------------------------------------------------
ground_alt = gps.altitude;
press_alt = gps.altitude; // Set initial value for filter
//save_pressure_data();
// Save Home to EEPROM
// -------------------
set_wp_with_index(home, 0);
// Save prev loc
// -------------
prev_WP = home;
}

466
Tools/ArduTracker/commands_process.pde
View File

@ -1,466 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// called by 10 Hz loop
// --------------------
void update_commands(void)
{
// This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX()
// ----------------------------------------------------------------
if((home_is_set == FALSE) || (control_mode != AUTO)){
return; // don't do commands
}
if(control_mode == AUTO){
load_next_command();
process_next_command();
}
//verify_must();
//verify_may();
}
void load_next_command()
{
// fetch next command if it's empty
// --------------------------------
if(next_command.id == CMD_BLANK){
next_command = get_wp_with_index(get(PARAM_WP_INDEX)+1);
if(next_command.id != CMD_BLANK){
//SendDebug("MSG <load_next_command> fetch found new cmd from list at index ");
//SendDebug((get(PARAM_WP_INDEX)+1),DEC);
//SendDebug(" with cmd id ");
//SendDebugln(next_command.id,DEC);
}
}
// If the preload failed, return or just Loiter
// generate a dynamic command for RTL
// --------------------------------------------
if(next_command.id == CMD_BLANK){
// we are out of commands!
gcs.send_text(SEVERITY_LOW,"out of commands!");
//SendDebug("MSG <load_next_command> out of commands, get(PARAM_WP_INDEX): ");
//SendDebugln(get(PARAM_WP_INDEX),DEC);
switch (control_mode){
case LAND:
// don't get a new command
break;
default:
next_command = get_LOITER_home_wp();
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
//SendDebugln(next_command.id,DEC);
break;
}
}
}
void process_next_command()
{
// these are waypoint/Must commands
// ---------------------------------
if (command_must_index == 0){ // no current command loaded
if (next_command.id >= 0x10 && next_command.id <= 0x1F ){
increment_WP_index();
//save_command_index(); // to Recover from in air Restart
command_must_index = get(PARAM_WP_INDEX);
//SendDebug("MSG <process_next_command> new command_must_id ");
//SendDebug(next_command.id,DEC);
//SendDebug(" index:");
//SendDebugln(command_must_index,DEC);
if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD)
Log_Write_Cmd(get(PARAM_WP_INDEX), &next_command);
process_must();
}
}
// these are May commands
// ----------------------
if (command_may_index == 0){
if (next_command.id >= 0x20 && next_command.id <= 0x2F ){
increment_WP_index();// this command is from the command list in EEPROM
command_may_index = get(PARAM_WP_INDEX);
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD)
Log_Write_Cmd(get(PARAM_WP_INDEX), &next_command);
process_may();
}
}
// these are do it now commands
// ---------------------------
if (next_command.id >= 0x30){
increment_WP_index();// this command is from the command list in EEPROM
if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD)
Log_Write_Cmd(get(PARAM_WP_INDEX), &next_command);
process_now();
}
}
/*
These functions implement the waypoint commands.
*/
void process_must()
{
//SendDebug("process must index: ");
//SendDebugln(command_must_index,DEC);
gcs.send_text(SEVERITY_LOW,"New cmd: <process_must>");
gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX));
// clear May indexes
command_may_index = 0;
command_may_ID = 0;
command_must_ID = next_command.id;
// loads the waypoint into Next_WP struct
// --------------------------------------
set_next_WP(&next_command);
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
// reset navigation integrators
// -------------------------
reset_I();
switch(command_must_ID){
case CMD_TAKEOFF: // TAKEOFF!
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch = (int)next_command.p1 * 100;
takeoff_altitude = (int)next_command.alt;
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
break;
case CMD_WAYPOINT: // Navigate to Waypoint
break;
case CMD_LAND: // LAND to Waypoint
break;
case CMD_LOITER: // Loiter indefinitely
break;
case CMD_LOITER_N_TURNS: // Loiter N Times
loiter_total = next_command.p1 * 360;
break;
case CMD_LOITER_TIME:
loiter_time = millis();
loiter_time_max = next_command.p1 * 10000; // seconds * 10 * 1000
break;
case CMD_RTL:
break;
}
}
void process_may()
{
//Serial.print("process_may cmd# ");
//Serial.println(get(PARAM_WP_INDEX),DEC);
command_may_ID = next_command.id;
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX));
// do the command
// --------------
switch(command_may_ID){
case CMD_DELAY: // Navigate to Waypoint
delay_start = millis();
delay_timeout = next_command.lat;
break;
case CMD_LAND_OPTIONS: // Land this puppy
gcs.send_text(SEVERITY_LOW,"Landing options set");
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
landing_pitch = next_command.lng * 100;
set(PARAM_TRIM_AIRSPEED, next_command.alt * 100);
set(PARAM_TRIM_THROTTLE,next_command.lat);
landing_distance = next_command.p1;
//landing_roll = command.lng;
SendDebug("MSG: throttle_cruise = ");
SendDebugln(get(PARAM_TRIM_THROTTLE),DEC);
break;
default:
break;
}
}
void process_now()
{
const float t5 = 100000.0;
//Serial.print("process_now cmd# ");
//Serial.println(get(PARAM_WP_INDEX),DEC);
byte id = next_command.id;
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
gcs.send_message(MSG_COMMAND_LIST, get(PARAM_WP_INDEX));
// do the command
// --------------
switch(id){
//case CMD_AP_MODE:
//next_command.p1 = constrain(next_command.p1, MANUAL, LAND);
//set_mode(next_command.p1);
//break;
case CMD_RESET_INDEX:
init_commands();
break;
case CMD_GETVAR_INDEX:
//
break;
case CMD_SENDVAR_INDEX:
//
break;
case CMD_TELEMETRY:
//
break;
case CMD_AIRSPEED_CRUISE:
set(PARAM_TRIM_AIRSPEED,next_command.p1 * 100);
break;
case CMD_THROTTLE_CRUISE:
set(PARAM_TRIM_THROTTLE,next_command.p1);
break;
case CMD_RESET_HOME:
init_home();
break;
case CMD_KP_GAIN:
pid_index[next_command.p1]->kP(next_command.alt/t5);
pid_index[next_command.p1]->save_gains();
break;
case CMD_KI_GAIN:
pid_index[next_command.p1]->kI(next_command.alt/t5);
pid_index[next_command.p1]->save_gains();
break;
case CMD_KD_GAIN:
pid_index[next_command.p1]->kD(next_command.alt/t5);
pid_index[next_command.p1]->save_gains();
break;
case CMD_KI_MAX:
pid_index[next_command.p1]->imax(next_command.alt/t5);
pid_index[next_command.p1]->save_gains();
break;
case CMD_KFF_GAIN:
{
float val = next_command.alt/t5;
switch(next_command.p1)
{
case CASE_PITCH_COMP: set(PARAM_KFF_PTCHCOMP,val); break;
case CASE_RUDDER_MIX: set(PARAM_KFF_RDDRMIX,val); break;
case CASE_P_TO_T: set(PARAM_KFF_PTCH2THR,val); break;
}
}
break;
case CMD_RADIO_TRIM:
set_radio_trim(next_command.p1,next_command.alt);
break;
case CMD_RADIO_MAX:
set_radio_max(next_command.p1,next_command.alt);
break;
case CMD_RADIO_MIN:
set_radio_min(next_command.p1,next_command.alt);
break;
case CMD_REPEAT:
new_event(&next_command);
break;
case CMD_SERVO:
//Serial.print("CMD_SERVO ");
//Serial.print(next_command.p1,DEC);
//Serial.print(" PWM: ");
//Serial.println(next_command.alt,DEC);
APM_RC.OutputCh(next_command.p1, next_command.alt);
break;
case CMD_INDEX:
command_must_index = 0;
command_may_index = 0;
set(PARAM_WP_INDEX,next_command.p1 - 1);
break;
case CMD_RELAY:
if (next_command.p1 == 0) {
relay_on();
} else if (next_command.p1 == 1) {
relay_off();
}else{
relay_toggle();
}
break;
}
}
// Verify commands
// ---------------
void verify_must()
{
float power;
switch(command_must_ID) {
case CMD_TAKEOFF: // Takeoff!
//Serial.print("verify_must cmd# ");
//Serial.println(command_must_index,DEC);
if (gps.ground_speed > 300){
if(hold_course == -1){
// save our current course to take off
#if MAGNETOMETER == ENABLED
hold_course = dcm.yaw_sensor;
#else
hold_course = gps.ground_course;
#endif
}
}
if(hold_course > -1){
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
calc_bearing_error();
}
if (current_loc.alt > (home.alt + takeoff_altitude)) {
command_must_index = 0;
hold_course = -1;
takeoff_complete = true;
}
break;
case CMD_LAND:
// we don't verify landing - we never go to a new Must command after Land.
if ( ((wp_distance > 0) && (wp_distance <= (2*gps.ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300) )
{
land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude
if(hold_course == -1){
// save our current course to land
//hold_course = yaw_sensor;
// save the course line of the runway to land
hold_course = crosstrack_bearing;
}
}
if(hold_course > -1){
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
calc_bearing_error();
}
update_crosstrack();
break;
case CMD_WAYPOINT: // reach a waypoint
hold_course == -1;
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= get(PARAM_WP_RADIUS))) {
SendDebug("MSG <verify_must: CMD_WAYPOINT> REACHED_WAYPOINT #");
SendDebugln(command_must_index,DEC);
// clear the command queue;
command_must_index = 0;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
gcs.send_text(SEVERITY_MEDIUM,"<verify_must: CMD_WAYPOINT> Missed WP");
command_must_index = 0;
}
break;
case CMD_LOITER_N_TURNS: // LOITER N times
update_loiter();
calc_bearing_error();
if(loiter_sum > loiter_total) {
loiter_total = 0;
gcs.send_text(SEVERITY_LOW,"<verify_must: CMD_LOITER_N_TURNS> LOITER orbits complete ");
// clear the command queue;
command_must_index = 0;
}
break;
case CMD_LOITER_TIME: // loiter N milliseconds
update_loiter();
calc_bearing_error();
if ((millis() - loiter_time) > loiter_time_max) {
gcs.send_text(SEVERITY_LOW,"<verify_must: CMD_LOITER_TIME> LOITER time complete ");
command_must_index = 0;
}
break;
case CMD_RTL:
if (wp_distance <= 30) {
//Serial.println("REACHED_HOME");
command_must_index = 0;
}
break;
case CMD_LOITER: // Just plain LOITER
update_loiter();
calc_bearing_error();
break;
default:
gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
break;
}
}
void verify_may()
{
switch(command_may_ID) {
case CMD_DELAY:
if ((millis() - delay_start) > delay_timeout){
command_may_index = 0;
delay_timeout = 0;
}
case CMD_LAND_OPTIONS:
if ((wp_distance > 0) && (wp_distance <= landing_distance)) {
//Serial.print("XXX REACHED_WAYPOINT #");
//Serial.println(command_must_index,DEC);
// clear the command queue;
command_may_index = 0;
}
break;
}
}

609
Tools/ArduTracker/config.h
View File

@ -1,609 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
//
// DO NOT EDIT this file to adjust your configuration. Create your own
// APM_Config.h and use APM_Config.h.example as a reference.
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
///
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Default and automatic configuration details.
//
// Notes for maintainers:
//
// - Try to keep this file organised in the same order as APM_Config.h.example
//
#include "defines.h"
///
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
/// change in your local copy of APM_Config.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
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_SENSOR
// AIRSPEED_RATIO
//
#ifndef AIRSPEED_SENSOR
# define AIRSPEED_SENSOR DISABLED
#endif
#ifndef AIRSPEED_RATIO
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_PROTOCOL OPTIONAL
// HIL_MODE OPTIONAL
// HIL_PORT OPTIONAL
#if HIL_MODE != HIL_MODE_DISABLED
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#ifndef HIL_PROTOCOL
# error Must define HIL_PROTOCOL if HIL_MODE is not disabled
#endif
#ifndef HIL_PORT
# error Must define HIL_PORT if HIL_PROTOCOL is set
#endif
#endif
#if HIL_MODE != HIL_MODE_DISABLED
// check xplane settings
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
// MAGNETOMETER not supported by XPLANE
# undef MAGNETOMETER
# define MAGNETOMETER DISABLED
# if HIL_MODE != HIL_MODE_ATTITUDE
# error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE
# endif
#endif
#endif // HIL_MODE != HIL_MODE_DISABLED
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# error XXX
# error XXX You must define GPS_PROTOCOL in APM_Config.h, or select a HIL configuration.
# error XXX
#endif
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL
// GCS_PORT
//
#ifndef GCS_PROTOCOL
# define GCS_PROTOCOL GCS_PROTOCOL_NONE
#endif
#ifndef DEBUGTERMINAL_VERBOSE
# define DEBUGTERMINAL_VERBOSE 1 //Set this to 1 to get more detail in DEBUGTERMINAL messages, or 0 to save flash space
#endif
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
#ifndef SERIAL0_BAUD
# define SERIAL0_BAUD 115200
#endif
#ifndef SERIAL3_BAUD
# define SERIAL3_BAUD 57600
#endif
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef BATTERY_EVENT
# define BATTERY_EVENT DISABLED
#endif
#ifndef BATTERY_TYPE
# define BATTERY_TYPE 0
#endif
#ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 11.4
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.0
#endif
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE
//
#ifndef INPUT_VOLTAGE
# define INPUT_VOLTAGE 5.0
#endif
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
// NOTE - There is no support for using the magnetometer in v1.0
#ifndef MAGNETOMETER
# define MAGNETOMETER DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Radio channel limits
//
// Note that these are not called out in APM_Config.h.example as there
// is currently no configured behaviour for these channels.
//
#ifndef CH5_MIN
# define CH5_MIN 1000
#endif
#ifndef CH5_MAX
# define CH5_MAX 2000
#endif
#ifndef CH6_MIN
# define CH6_MIN 1000
#endif
#ifndef CH6_MAX
# define CH6_MAX 2000
#endif
#ifndef CH7_MIN
# define CH7_MIN 1000
#endif
#ifndef CH7_MAX
# define CH7_MAX 2000
#endif
#ifndef CH8_MIN
# define CH8_MIN 1000
#endif
#ifndef CH8_MAX
# define CH8_MAX 2000
#endif
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE
// FLIGHT_MODE_CHANNEL
//
#ifndef FLIGHT_MODE_CHANNEL
# define FLIGHT_MODE_CHANNEL 8
#endif
#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8)
# error XXX
# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8
# error XXX
#endif
#if !defined(FLIGHT_MODE_1)
# define FLIGHT_MODE_1 FLY_BY_WIRE_A
#endif
#if !defined(FLIGHT_MODE_2)
# define FLIGHT_MODE_2 FLY_BY_WIRE_A
#endif
#if !defined(FLIGHT_MODE_3)
# define FLIGHT_MODE_3 STABILIZE
#endif
#if !defined(FLIGHT_MODE_4)
# define FLIGHT_MODE_4 STABILIZE
#endif
#if !defined(FLIGHT_MODE_5)
# define FLIGHT_MODE_5 MANUAL
#endif
#if !defined(FLIGHT_MODE_6)
# define FLIGHT_MODE_6 MANUAL
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE
// THROTTLE_FS_VALUE
// THROTTLE_FAILSAFE_ACTION
//
#ifndef THROTTLE_FAILSAFE
# define THROTTLE_FAILSAFE DISABLED
#endif
#ifndef THROTTE_FS_VALUE
# define THROTTLE_FS_VALUE 975
#endif
#ifndef THROTTLE_FAILSAFE_ACTION
# define THROTTLE_FAILSAFE_ACTION 2
#endif
//////////////////////////////////////////////////////////////////////////////
// AUTO_TRIM
//
#ifndef AUTO_TRIM
# define AUTO_TRIM ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_REVERSE
//
#ifndef THROTTLE_REVERSE
# define THROTTLE_REVERSE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_STICK_MIXING
//
#ifndef ENABLE_STICK_MIXING
# define ENABLE_STICK_MIXING ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_OUT
//
#ifndef THROTTE_OUT
# define THROTTLE_OUT ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// STARTUP BEHAVIOUR
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GROUND_START_DELAY
//
#ifndef GROUND_START_DELAY
# define GROUND_START_DELAY 0
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_AIR_START
//
#ifndef ENABLE_AIR_START
# define ENABLE_AIR_START DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE REVERSE_SWITCH
//
#ifndef REVERSE_SWITCH
# define REVERSE_SWITCH ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Altitude measurement and control.
//
#ifndef AOA
# define AOA 100 // XXX still 100ths of a degree
#endif
#ifndef ALT_EST_GAIN
# define ALT_EST_GAIN 0.01
#endif
#ifndef ALTITUDE_MIX
# define ALTITUDE_MIX 0
#endif
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_CRUISE
//
#ifndef AIRSPEED_CRUISE
# define AIRSPEED_CRUISE 1000 // 10 m/s * 100
#endif
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control
//
#ifndef AIRSPEED_FBW_MIN
# define AIRSPEED_FBW_MIN 6
#endif
#ifndef AIRSPEED_FBW_MAX
# define AIRSPEED_FBW_MAX 30
#endif
#ifndef THROTTLE_ALT_P
# define THROTTLE_ALT_P 0.32
#endif
#ifndef THROTTLE_ALT_I
# define THROTTLE_ALT_I 0.0
#endif
#ifndef THROTTLE_ALT_D
# define THROTTLE_ALT_D 0.0
#endif
#ifndef THROTTLE_ALT_INT_MAX
# define THROTTLE_ALT_INT_MAX 20
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control
//
#ifndef THROTTLE_MIN
# define THROTTLE_MIN 0
#endif
#ifndef THROTTLE_CRUISE
# define THROTTLE_CRUISE 45
#endif
#ifndef THROTTLE_MAX
# define THROTTLE_MAX 75
#endif
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
#ifndef HEAD_MAX
# define HEAD_MAX 4500 // deg * 100
#endif
#ifndef PITCH_MAX
# define PITCH_MAX 1500 // deg * 100
#endif
#ifndef PITCH_MIN
# define PITCH_MIN -2500 // deg * 100
#endif
//////////////////////////////////////////////////////////////////////////////
// Attitude control gains
//
#ifndef SERVO_ROLL_P
# define SERVO_ROLL_P 0.4
#endif
#ifndef SERVO_ROLL_I
# define SERVO_ROLL_I 0.0
#endif
#ifndef SERVO_ROLL_D
# define SERVO_ROLL_D 0.0
#endif
#ifndef SERVO_ROLL_INT_MAX
# define SERVO_ROLL_INT_MAX 5
#endif
#ifndef ROLL_SLEW_LIMIT
# define ROLL_SLEW_LIMIT 0
#endif
#ifndef SERVO_PITCH_P
# define SERVO_PITCH_P 0.6
#endif
#ifndef SERVO_PITCH_I
# define SERVO_PITCH_I 0.0
#endif
#ifndef SERVO_PITCH_D
# define SERVO_PITCH_D 0.0
#endif
#ifndef SERVO_PITCH_INT_MAX
# define SERVO_PITCH_INT_MAX 5
#endif
#ifndef PITCH_COMP
# define PITCH_COMP 0.2
#endif
#ifndef SERVO_YAW_P
# define SERVO_YAW_P 0.0
#endif
#ifndef SERVO_YAW_I
# define SERVO_YAW_I 0.0
#endif
#ifndef SERVO_YAW_D
# define SERVO_YAW_D 0.0
#endif
#ifndef SERVO_YAW_INT_MAX
# define SERVO_YAW_INT_MAX 5
#endif
#ifndef RUDDER_MIX
# define RUDDER_MIX 0.5
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
#ifndef NAV_ROLL_P
# define NAV_ROLL_P 0.7
#endif
#ifndef NAV_ROLL_I
# define NAV_ROLL_I 0.0
#endif
#ifndef NAV_ROLL_D
# define NAV_ROLL_D 0.02
#endif
#ifndef NAV_ROLL_INT_MAX
# define NAV_ROLL_INT_MAX 5
#endif
#ifndef NAV_PITCH_ASP_P
# define NAV_PITCH_ASP_P 0.65
#endif
#ifndef NAV_PITCH_ASP_I
# define NAV_PITCH_ASP_I 0.0
#endif
#ifndef NAV_PITCH_ASP_D
# define NAV_PITCH_ASP_D 0.0
#endif
#ifndef NAV_PITCH_ASP_INT_MAX
# define NAV_PITCH_ASP_INT_MAX 5
#endif
#ifndef NAV_PITCH_ALT_P
# define NAV_PITCH_ALT_P 0.65
#endif
#ifndef NAV_PITCH_ALT_I
# define NAV_PITCH_ALT_I 0.0
#endif
#ifndef NAV_PITCH_ALT_D
# define NAV_PITCH_ALT_D 0.0
#endif
#ifndef NAV_PITCH_ALT_INT_MAX
# define NAV_PITCH_ALT_INT_MAX 5
#endif
//////////////////////////////////////////////////////////////////////////////
// Energy/Altitude control gains
//
#ifndef THROTTLE_TE_P
# define THROTTLE_TE_P 0.50
#endif
#ifndef THROTTLE_TE_I
# define THROTTLE_TE_I 0.0
#endif
#ifndef THROTTLE_TE_D
# define THROTTLE_TE_D 0.0
#endif
#ifndef THROTTLE_TE_INT_MAX
# define THROTTLE_TE_INT_MAX 20
#endif
#ifndef THROTTLE_SLEW_LIMIT
# define THROTTLE_SLEW_LIMIT 0
#endif
#ifndef P_TO_T
# define P_TO_T 2.5
#endif
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 10 // deg/m * 100
#endif
#ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 3000 // deg * 100
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUGGING
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUG_SUBSYSTEM
//
#ifndef DEBUG_SUBSYSTEM
# define DEBUG_SUBSYSTEM 0
#endif
//////////////////////////////////////////////////////////////////////////////
// DEBUG_LEVEL
//
#ifndef DEBUG_LEVEL
# define DEBUG_LEVEL SEVERITY_LOW
#endif
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED
#endif
#ifndef LOG_ATTITUDE_MED
# define LOG_ATTITUDE_MED ENABLED
#endif
#ifndef LOG_GPS
# define LOG_GPS ENABLED
#endif
#ifndef LOG_PM
# define LOG_PM ENABLED
#endif
#ifndef LOG_CTUN
# define LOG_CTUN DISABLED
#endif
#ifndef LOG_NTUN
# define LOG_NTUN DISABLED
#endif
#ifndef LOG_MODE
# define LOG_MODE ENABLED
#endif
#ifndef LOG_RAW
# define LOG_RAW DISABLED
#endif
#ifndef LOG_CMD
# define LOG_CMD ENABLED
#endif
#ifndef DEBUG_PORT
# define DEBUG_PORT 0
#endif
#if DEBUG_PORT == 0
# define SendDebug_P(a) Serial.print_P(PSTR(a))
# define SendDebugln_P(a) Serial.println_P(PSTR(a))
# define SendDebug Serial.print
# define SendDebugln Serial.println
#elif DEBUG_PORT == 1
# define SendDebug_P(a) Serial1.print_P(PSTR(a))
# define SendDebugln_P(a) Serial1.println_P(PSTR(a))
# define SendDebug Serial1.print
# define SendDebugln Serial1.println
#elif DEBUG_PORT == 2
# define SendDebug_P(a) Serial2.print_P(PSTR(a))
# define SendDebugln_P(a) Serial2.println_P(PSTR(a))
# define SendDebug Serial2.print
# define SendDebugln Serial2.println
#elif DEBUG_PORT == 3
# define SendDebug_P(a) Serial3.print_P(PSTR(a))
# define SendDebugln_P(a) Serial3.println_P(PSTR(a))
# define SendDebug Serial3.print
# define SendDebugln Serial3.println
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation defaults
//
#ifndef WP_RADIUS_DEFAULT
# define WP_RADIUS_DEFAULT 20
#endif
#ifndef LOITER_RADIUS_DEFAULT
# define LOITER_RADIUS_DEFAULT 60
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
#ifndef STANDARD_SPEED
# define STANDARD_SPEED 15.0
#endif
#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
#define THROTTLE2SPEED_SCALER

48
Tools/ArduTracker/control_modes.pde
View File

@ -1,48 +0,0 @@
void read_control_switch()
{
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
set_mode(flight_mode(switchPosition));
oldSwitchPosition = switchPosition;
// reset navigation integrators
// -------------------------
reset_I();
}
}
byte readSwitch(void){
uint16_t pulsewidth = APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH) - 1);
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
if (pulsewidth > 1490 && pulsewidth <= 1620) return 3;
if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual
if (pulsewidth >= 1750) return 5; // Hardware Manual
return 0;
}
void reset_control_switch()
{
oldSwitchPosition = 0;
read_control_switch();
SendDebug("MSG: reset_control_switch");
SendDebugln(oldSwitchPosition , DEC);
}
void update_servo_switches()
{
// up is reverse
// up is elevon
mix_mode = (PINL & 128) ? 1 : 0;
if (mix_mode == 0) {
reverse_roll = (PINE & 128) ? 1 : -1;
reverse_pitch = (PINE & 64) ? 1 : -1;
reverse_rudder = (PINL & 64) ? 1 : -1;
} else {
reverse_elevons = (PINE & 128) ? 1 : -1;
reverse_ch1_elevon = (PINE & 64) ? 1 : -1;
reverse_ch2_elevon = (PINL & 64) ? 1 : -1;
}
}

68
Tools/ArduTracker/createTags
View File

@ -1,68 +0,0 @@
#!/bin/bash
#" Autocompletion enabled vim for arduino pde's
ctags -RV --language-force=C++ --c++-kinds=+p --fields=+iaS --extra=+q \
. \
../libraries/* \
/usr/lib/avr/include
# sample vimrc file
# you'll need to have omnicpp plugin for vim
#"set compatible
#" Vim5 and later versions support syntax highlighting. Uncommenting the next
#" line enables syntax highlighting by default.
#syntax on
#filetype on
#au BufNewFile,BufRead *.pde set filetype=cpp
#" If using a dark background within the editing area and syntax highlighting
#" turn on this option as well
#"set background=dark
#" Uncomment the following to have Vim jump to the last position when
#" reopening a file
#if has("autocmd")
#au BufReadPost * if line("'\"") > 1 && line("'\"") <= line("$") | exe "normal! g'\"" | endif
#endif
#" Uncomment the following to have Vim load indentation rules and plugins
#" according to the detected filetype.
#if has("autocmd")
#filetype plugin indent on
#endif
#" The following are commented out as they cause vim to behave a lot
#" differently from regular Vi. They are highly recommended though.
#set showcmd " Show (partial) command in status line.
#set showmatch " Show matching brackets.
#set ignorecase " Do case insensitive matching
#set smartcase " Do smart case matching
#set incsearch " Incremental search
#set autowrite " Automatically save before commands like :next and :make
#set hidden " Hide buffers when they are abandoned
#set mouse=a " Enable mouse usage (all modes)
#set vb
#" build tags of your own project with Ctrl-F12
#map <C-F12> :!ctags -R --sort=yes --c++-kinds=+p --fields=+iaS --extra=+q .<CR>
#" OmniCppComplete
#let OmniCpp_NamespaceSearch = 1
#let OmniCpp_GlobalScopeSearch = 1
#let OmniCpp_ShowAccess = 1
#let OmniCpp_ShowPrototypeInAbbr = 1 " show function parameters
#let OmniCpp_MayCompleteDot = 1 " autocomplete after .
#let OmniCpp_MayCompleteArrow = 1 " autocomplete after ->
#let OmniCpp_MayCompleteScope = 1 " autocomplete after ::
#let OmniCpp_DefaultNamespaces = ["std", "_GLIBCXX_STD"]
#" arduino syntax
#au BufNewFile,BufRead *.pde setf arduino
#" automatically open and close the popup menu / preview window
#"au CursorMovedI,InsertLeave * if pumvisible() == 0|silent! pclose|endif
#"set completeopt=menuone,menu,longest,preview
#set ts=4
#set sw=4

70
Tools/ArduTracker/debug.pde
View File

@ -1,70 +0,0 @@
#if DEBUG_SUBSYSTEM == 1
void debug_subsystem()
{
Serial.println("GPS Subsystem, RAW OUTPUT");
while(1){
if(Serial1.available() > 0) // Ok, let me see, the buffer is empty?
{
delay(60); // wait for it all
while(Serial1.available() > 0){
byte incoming = Serial1.read();
//Serial.print(incoming,DEC);
Serial.print(incoming, BYTE); // will output Hex values
//Serial.print(",");
}
Serial.println(" ");
}
}
}
#endif
#if DEBUG_SUBSYSTEM == 2
void debug_subsystem()
{
Serial.println("Control Switch");
Serial.print("Control CH ");
Serial.println(flight_mode_channel, DEC);
while(1){
delay(20);
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
Serial.printf_P(PSTR("Position %d\n"), i, switchPosition);
oldSwitchPosition = switchPosition;
}
}
}
#endif
#if DEBUG_SUBSYSTEM == 3
void debug_subsystem()
{
Serial.println("DIP Switch Test");
while(1){
delay(100);
update_servo_switches();
if (mix_mode == 0) {
Serial.print("Mix:standard ");
Serial.print("\t reverse_roll: ");
Serial.print(reverse_roll, DEC);
Serial.print("\t reverse_pitch: ");
Serial.print(reverse_pitch, DEC);
Serial.print("\t reverse_rudder: ");
Serial.println(reverse_rudder, DEC);
} else {
Serial.print("Mix:elevons ");
Serial.print("\t reverse_elevons: ");
Serial.print(reverse_elevons, DEC);
Serial.print("\t reverse_ch1_elevon: ");
Serial.print(reverse_ch1_elevon, DEC);
Serial.print("\t reverse_ch2_elevon: ");
Serial.println(reverse_ch2_elevon, DEC);
}
}
}
#endif

312
Tools/ArduTracker/defines.h
View File

@ -1,312 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Internal defines, don't edit and expect things to work
// -------------------------------------------------------
#define TRUE 1
#define FALSE 0
#define ToRad(x) radians(x) // *pi/180
#define ToDeg(x) degrees(x) // *180/pi
#define DEBUG 0
#define LOITER_RANGE 30 // for calculating power outside of loiter radius
// GPS baud rates
// --------------
#define NO_GPS 38400
#define NMEA_GPS 38400
#define EM406_GPS 57600
#define UBLOX_GPS 38400
#define ARDU_IMU 38400
#define MTK_GPS 38400
#define SIM_GPS 38400
// GPS type codes - use the names, not the numbers
#define GPS_PROTOCOL_NONE -1
#define GPS_PROTOCOL_NMEA 0
#define GPS_PROTOCOL_SIRF 1
#define GPS_PROTOCOL_UBLOX 2
#define GPS_PROTOCOL_IMU 3
#define GPS_PROTOCOL_MTK 4
#define GPS_PROTOCOL_HIL 5
#define GPS_PROTOCOL_MTK19 6
#define GPS_PROTOCOL_AUTO 7
// Radio channels
// Note channels are from 0!
//
// XXX these should be CH_n defines from RC.h at some point.
#define CH_ROLL 0
#define CH_PITCH 1
#define CH_THROTTLE 2
#define CH_RUDDER 3
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#define WP_SIZE 14
// HIL enumerations
#define HIL_PROTOCOL_XPLANE 1
#define HIL_PROTOCOL_MAVLINK 2
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
// GCS enumeration
#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol
#define GCS_PROTOCOL_LEGACY 1 // legacy ArduPilot protocol
#define GCS_PROTOCOL_XPLANE 2 // X-Plane HIL simulation
#define GCS_PROTOCOL_DEBUGTERMINAL 3 //Human-readable debug interface for use with a dumb terminal
#define GCS_PROTOCOL_MAVLINK 4 // binary protocol for qgroundcontrol
#define GCS_PROTOCOL_NONE -1 // No GCS output
// PID enumeration
// ---------------
#define CASE_SERVO_ROLL 0
#define CASE_SERVO_PITCH 1
#define CASE_SERVO_RUDDER 2
#define CASE_NAV_ROLL 3
#define CASE_NAV_PITCH_ASP 4
#define CASE_NAV_PITCH_ALT 5
#define CASE_TE_THROTTLE 6
#define CASE_ALT_THROTTLE 7
// Feedforward cases
// ----------------
#define CASE_PITCH_COMP 0
#define CASE_RUDDER_MIX 1
#define CASE_P_TO_T 2
// Auto Pilot modes
// ----------------
#define MANUAL 0
#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle
#define STABILIZE 2
#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle
#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed
// Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0
#define AUTO 10
#define RTL 11
#define LOITER 12
#define TAKEOFF 13
#define LAND 14
// Command IDs - Must
#define CMD_BLANK 0x00 // there is no command stored in the mem location requested
#define CMD_WAYPOINT 0x10
#define CMD_LOITER 0x11
#define CMD_LOITER_N_TURNS 0x12
#define CMD_LOITER_TIME 0x13
#define CMD_RTL 0x14
#define CMD_LAND 0x15
#define CMD_TAKEOFF 0x16
// Command IDs - May
#define CMD_DELAY 0x20
#define CMD_CLIMB 0x21 // NOT IMPLEMENTED
#define CMD_LAND_OPTIONS 0x22 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
// Command IDs - Now
//#define CMD_AP_MODE 0x30
#define CMD_RESET_INDEX 0x31
#define CMD_GOTO_INDEX 0x32 // NOT IMPLEMENTED
#define CMD_GETVAR_INDEX 0x33
#define CMD_SENDVAR_INDEX 0x34
#define CMD_TELEMETRY 0x35
#define CMD_THROTTLE_CRUISE 0x40
#define CMD_AIRSPEED_CRUISE 0x41
#define CMD_RESET_HOME 0x44
#define CMD_KP_GAIN 0x60
#define CMD_KI_GAIN 0x61
#define CMD_KD_GAIN 0x62
#define CMD_KI_MAX 0x63
#define CMD_KFF_GAIN 0x64
#define CMD_RADIO_TRIM 0x70
#define CMD_RADIO_MAX 0x71
#define CMD_RADIO_MIN 0x72
#define CMD_RADIO_MIN 0x72
#define CMD_ELEVON_TRIM 0x73
#define CMD_INDEX 0x75 // sets the current Must index
#define CMD_REPEAT 0x80
#define CMD_RELAY 0x81
#define CMD_SERVO 0x82 // move servo N to PWM value
//repeating events
#define NO_REPEAT 0
#define CH_4_TOGGLE 1
#define CH_5_TOGGLE 2
#define CH_6_TOGGLE 3
#define CH_7_TOGGLE 4
#define RELAY_TOGGLE 5
#define STOP_REPEAT 10
// GCS Message ID's
#define MSG_ACKNOWLEDGE 0x00
#define MSG_HEARTBEAT 0x01
#define MSG_ATTITUDE 0x02
#define MSG_LOCATION 0x03
#define MSG_PRESSURE 0x04
#define MSG_STATUS_TEXT 0x05
#define MSG_PERF_REPORT 0x06
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
#define MSG_VERSION_REQUEST 0x08
#define MSG_VERSION 0x09
#define MSG_EXTENDED_STATUS 0x0a
#define MSG_CPU_LOAD 0x0b
#define MSG_COMMAND_REQUEST 0x20
#define MSG_COMMAND_UPLOAD 0x21
#define MSG_COMMAND_LIST 0x22
#define MSG_COMMAND_MODE_CHANGE 0x23
#define MSG_CURRENT_WAYPOINT 0x24
#define MSG_VALUE_REQUEST 0x30
#define MSG_VALUE_SET 0x31
#define MSG_VALUE 0x32
#define MSG_PID_REQUEST 0x40
#define MSG_PID_SET 0x41
#define MSG_PID 0x42
#define MSG_TRIM_STARTUP 0x50
#define MSG_TRIM_MIN 0x51
#define MSG_TRIM_MAX 0x52
#define MSG_RADIO_OUT 0x53
#define MSG_RAW_IMU 0x60
#define MSG_GPS_STATUS 0x61
#define MSG_GPS_RAW 0x62
#define MSG_AIRSPEED 0x63
#define MSG_SERVO_OUT 0x70
#define MSG_PIN_REQUEST 0x80
#define MSG_PIN_SET 0x81
#define MSG_DATAFLASH_REQUEST 0x90
#define MSG_DATAFLASH_SET 0x91
#define MSG_EEPROM_REQUEST 0xa0
#define MSG_EEPROM_SET 0xa1
#define MSG_POSITION_CORRECT 0xb0
#define MSG_ATTITUDE_CORRECT 0xb1
#define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4
#define SEVERITY_LOW 1
#define SEVERITY_MEDIUM 2
#define SEVERITY_HIGH 3
#define SEVERITY_CRITICAL 4
// Logging parameters
#define LOG_ATTITUDE_MSG 0x01
#define LOG_GPS_MSG 0x02
#define LOG_MODE_MSG 0X03
#define LOG_CONTROL_TUNING_MSG 0X04
#define LOG_NAV_TUNING_MSG 0X05
#define LOG_PERFORMANCE_MSG 0X06
#define LOG_RAW_MSG 0x07
#define LOG_CMD_MSG 0x08
#define LOG_STARTUP_MSG 0x09
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
#define MASK_LOG_ATTITUDE_FAST 0
#define MASK_LOG_ATTITUDE_MED 2
#define MASK_LOG_GPS 4
#define MASK_LOG_PM 8
#define MASK_LOG_CTUN 16
#define MASK_LOG_NTUN 32
#define MASK_LOG_MODE 64
#define MASK_LOG_RAW 128
#define MASK_LOG_CMD 256
// Yaw modes
#define YAW_MODE_COORDINATE_TURNS 0
#define YAW_MODE_HOLD_HEADING 1
#define YAW_MODE_SLIP 2
// Waypoint Modes
// ----------------
#define ABS_WP 0
#define REL_WP 1
// Command Queues
// ---------------
#define COMMAND_MUST 0
#define COMMAND_MAY 1
#define COMMAND_NOW 2
// Events
// ------
#define EVENT_WILL_REACH_WAYPOINT 1
#define EVENT_SET_NEW_WAYPOINT_INDEX 2
#define EVENT_LOADED_WAYPOINT 3
#define EVENT_LOOP 4
//GPS_fix
#define VALID_GPS 0x00
#define BAD_GPS 0x01
#define FAILED_GPS 0x03
// Climb rate calculations
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers
#define BATTERY_PIN2 1
#define BATTERY_PIN3 2
#define BATTERY_PIN4 3
#define RELAY_PIN 47
// Hardware Parameters
#define SLIDE_SWITCH_PIN 40
#define PUSHBUTTON_PIN 41
#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C
#define B_LED_PIN 36
#define C_LED_PIN 35
#define HOLD_ALT_ABOVE_HOME 8 // bitmask value
#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered
#define EEPROM_MAX_ADDR 4096
// log
#define EE_END_OF_PARAMS getAddress(PARAM_LAST_UINT32)
#define EE_LAST_LOG_PAGE EE_END_OF_PARAMS+1
#define EE_LAST_LOG_NUM EE_END_OF_PARAMS+3
#define EE_LOG_1_START EE_END_OF_PARAMS+5
#define WP_START_BYTE EE_END_OF_PARAMS+45 // where in memory home WP is stored + all other WP
// add 19 to account for log files
// bits in log_bitmask
#define LOGBIT_ATTITUDE_FAST (1<<0)
#define LOGBIT_ATTITUDE_MED (1<<1)
#define LOGBIT_GPS (1<<2)
#define LOGBIT_PM (1<<3)
#define LOGBIT_CTUN (1<<4)
#define LOGBIT_NTUN (1<<5)
#define LOGBIT_MODE (1<<6)
#define LOGBIT_RAW (1<<7)
#define LOGBIT_CMD (1<<8)

225
Tools/ArduTracker/events.pde
View File

@ -1,225 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
This event will be called when the failsafe changes
boolean failsafe reflects the current state
*/
void failsafe_event()
{
if (failsafe == true){
// This is how to handle a failsafe.
switch(control_mode)
{
case MANUAL: // First position
set_mode(STABILIZE);
break;
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
set_mode(RTL);
set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE);
case CIRCLE: // middle position
break;
case AUTO: // middle position
case LOITER: // middle position
if (get(PARAM_THR_FS_ACTION)== 1){
set_mode(RTL);
set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE);
}
// 2 = Stay in AUTO and ignore failsafe
break;
case RTL: // middle position
break;
default:
set_mode(RTL);
set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE);
break;
}
}else{
reset_I();
}
}
void low_battery_event(void)
{
gcs.send_text(SEVERITY_HIGH,"Low Battery!");
set_mode(RTL);
set(PARAM_TRIM_THROTTLE,THROTTLE_CRUISE);
}
/*
4 simultaneous events
int event_original - original time in seconds
int event_countdown - count down to zero
byte event_countdown - ID for what to do
byte event_countdown - how many times to loop, 0 = indefinite
byte event_value - specific information for an event like PWM value
byte counterEvent - undo the event if nescessary
count down each one
new event
undo_event
*/
void new_event(struct Location *cmd)
{
SendDebug("New Event Loaded ");
SendDebugln(cmd->p1,DEC);
if(cmd->p1 == STOP_REPEAT){
SendDebugln("STOP repeat ");
event_id = NO_REPEAT;
event_timer = -1;
undo_event = 0;
event_value = 0;
event_delay = 0;
event_repeat = 0; // convert seconds to millis
event_undo_value = 0;
repeat_forever = 0;
}else{
// reset the event timer
event_timer = millis();
event_id = cmd->p1;
event_value = cmd->alt;
event_delay = cmd->lat;
event_repeat = cmd->lng; // convert seconds to millis
event_undo_value = 0;
repeat_forever = (event_repeat == 0) ? 1:0;
}
/*
Serial.print("event_id: ");
Serial.println(event_id,DEC);
Serial.print("event_value: ");
Serial.println(event_value,DEC);
Serial.print("event_delay: ");
Serial.println(event_delay,DEC);
Serial.print("event_repeat: ");
Serial.println(event_repeat,DEC);
Serial.print("event_undo_value: ");
Serial.println(event_undo_value,DEC);
Serial.print("repeat_forever: ");
Serial.println(repeat_forever,DEC);
Serial.print("Event_timer: ");
Serial.println(event_timer,DEC);
*/
perform_event();
}
void perform_event()
{
if (event_repeat > 0){
event_repeat --;
}
switch(event_id) {
case CH_4_TOGGLE:
event_undo_value = readOutputCh(4);
APM_RC.OutputCh(4, event_value); // send to Servos
undo_event = 2;
break;
case CH_5_TOGGLE:
event_undo_value = readOutputCh(5);
APM_RC.OutputCh(5, event_value); // send to Servos
undo_event = 2;
break;
case CH_6_TOGGLE:
event_undo_value = readOutputCh(6);
APM_RC.OutputCh(6, event_value); // send to Servos
undo_event = 2;
break;
case CH_7_TOGGLE:
event_undo_value = readOutputCh(7);
APM_RC.OutputCh(7, event_value); // send to Servos
undo_event = 2;
event_undo_value = 1;
break;
case RELAY_TOGGLE:
event_undo_value = PORTL & B00000100 ? 1:0;
relay_toggle();
SendDebug("toggle relay ");
SendDebugln(PORTL,BIN);
undo_event = 2;
break;
}
}
void relay_on()
{
PORTL |= B00000100;
}
void relay_off()
{
PORTL &= ~B00000100;
}
void relay_toggle()
{
PORTL ^= B00000100;
}
void update_events()
{
// repeating events
if(undo_event == 1){
perform_event_undo();
undo_event = 0;
}else if(undo_event > 1){
undo_event --;
}
if(event_timer == -1)
return;
if((millis() - event_timer) > event_delay){
perform_event();
if(event_repeat > 0 || repeat_forever == 1){
event_repeat--;
event_timer = millis();
}else{
event_timer = -1;
}
}
}
void perform_event_undo()
{
switch(event_id) {
case CH_4_TOGGLE:
APM_RC.OutputCh(4, event_undo_value); // send to Servos
break;
case CH_5_TOGGLE:
APM_RC.OutputCh(5, event_undo_value); // send to Servos
break;
case CH_6_TOGGLE:
APM_RC.OutputCh(6, event_undo_value); // send to Servos
break;
case CH_7_TOGGLE:
APM_RC.OutputCh(7, event_undo_value); // send to Servos
break;
case RELAY_TOGGLE:
relay_toggle();
SendDebug("untoggle relay ");
SendDebugln(PORTL,BIN);
break;
}
}

58
Tools/ArduTracker/global_data.h
View File

@ -1,58 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef __GLOBAL_DATA_H
#define __GLOBAL_DATA_H
#include "param_table.h"
#define ONBOARD_PARAM_NAME_LENGTH 15
#define MAX_WAYPOINTS 20
#define FIRMWARE_VERSION 12 // <-- change on param data struct modifications
#ifdef __cplusplus
///
// global data structure
// This structure holds all the vehicle parameters.
// TODO: bring in varibles floating around in ArduPilotMega.pde
//
struct global_struct
{
// parameters
uint16_t parameter_i; // parameter index
uint16_t param_count; // number of params
// waypoints
uint16_t waypoint_request_i; // request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "
bool waypoint_sending; // currently in send process
bool waypoint_receiving; // currently receiving
uint16_t waypoint_count;
uint32_t waypoint_timelast_send; // milliseconds
uint32_t waypoint_timelast_receive; // milliseconds
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
float junk; //used to return a junk value for interface
// data stream rates
uint16_t streamRateRawSensors;
uint16_t streamRateExtendedStatus;
uint16_t streamRateRCChannels;
uint16_t streamRateRawController;
uint16_t streamRateRawSensorFusion;
uint16_t streamRatePosition;
uint16_t streamRateExtra1;
uint16_t streamRateExtra2;
uint16_t streamRateExtra3;
// struct constructor
global_struct();
} global_data;
extern "C" const char *param_nametab[];
#endif // __cplusplus
#endif // __GLOBAL_DATA_H

188
Tools/ArduTracker/global_data.pde
View File

@ -1,188 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//////////////////////////////////////////////////////////////////////
// parameter get/set functions
//////////////////////////////////////////////////////////////////////
// calculate memory starting location for eeprom
static uint16_t floatMemStart = 0;
static uint16_t uint8MemStart = floatMemStart + PARAM_FLOAT_COUNT * sizeof(float);
static uint16_t uint16MemStart = uint8MemStart + PARAM_UINT8_COUNT * sizeof(uint8_t);
static uint16_t int16MemStart = uint16MemStart + PARAM_UINT16_COUNT * sizeof(uint16_t);
static uint16_t uint32MemStart = int16MemStart + PARAM_INT16_COUNT * sizeof(int16_t);
// read parameters
static uint8_t get(uint8_param_t id) __attribute__((noinline));
static uint8_t get(uint8_param_t id)
{
return eeprom_read_byte((uint8_t *)((id-PARAM_FIRST_UINT8)*sizeof(uint8_t)+uint8MemStart));
}
static uint16_t get(uint16_param_t id) __attribute__((noinline));
static uint16_t get(uint16_param_t id)
{
return eeprom_read_word((uint16_t *)((id-PARAM_FIRST_UINT16)*sizeof(uint16_t)+uint16MemStart));
}
static int16_t get(int16_param_t id) __attribute__((noinline));
static int16_t get(int16_param_t id)
{
return (int16_t)eeprom_read_word((uint16_t *)((id-PARAM_FIRST_INT16)*sizeof(int16_t)+int16MemStart));
}
static float get(float_param_t id) __attribute__((noinline));
static float get(float_param_t id)
{
float value;
eeprom_read_block((void*)&value,(const void*)((id-PARAM_FIRST_FLOAT)*sizeof(float)+floatMemStart),sizeof(value));
return value;
}
static float get(uint32_param_t id) __attribute__((noinline));
static float get(uint32_param_t id)
{
uint32_t value;
eeprom_read_block((void*)&value,(const void*)((id-PARAM_FIRST_UINT32)*sizeof(uint32_t)+uint32MemStart),sizeof(value));
return value;
}
// write parameters
static void set(uint8_param_t id, uint8_t val) __attribute__((noinline));
static void set(uint8_param_t id, uint8_t val)
{
eeprom_write_byte((uint8_t *)((id-PARAM_FIRST_UINT8)*sizeof(uint8_t)+uint8MemStart),val);
}
static void set(uint16_param_t id, uint16_t val) __attribute__((noinline));
static void set(uint16_param_t id, uint16_t val)
{
eeprom_write_word((uint16_t *)((id-PARAM_FIRST_UINT16)*sizeof(uint16_t)+uint16MemStart),val);
}
static void set(int16_param_t id, int16_t val) __attribute__((noinline));
static void set(int16_param_t id, int16_t val)
{
eeprom_write_word((uint16_t *)((id-PARAM_FIRST_INT16)*sizeof(int16_t)+int16MemStart),(uint16_t)val);
}
static void set(float_param_t id, float val) __attribute__((noinline));
static void set(float_param_t id, float val)
{
eeprom_write_block((const void *)&val,(void *)((id-PARAM_FIRST_FLOAT)*sizeof(float)+floatMemStart),sizeof(val));
}
static void set(uint32_param_t id, uint32_t val) __attribute__((noinline));
static void set(uint32_param_t id, uint32_t val)
{
eeprom_write_block((const void *)&val,(void *)((id-PARAM_FIRST_UINT32)*sizeof(uint32_t)+uint32MemStart),sizeof(val));
}
static void setParamAsFloat(uint16_t id, float value)
{
if (id < PARAM_FIRST_UINT8) set((float_param_t)id,value);
else if (id < PARAM_FIRST_UINT16) set((uint8_param_t)id,(uint8_t)value);
else if (id < PARAM_FIRST_INT16) set((uint16_param_t)id,(uint16_t)value);
else if (id < PARAM_FIRST_UINT32) set((int16_param_t)id,(int16_t)value);
else if (id < PARAM_COUNT) set((uint32_param_t)id,(uint32_t)value);
// XXX: uint32 won't have full bitwise precision
}
static float getParamAsFloat(uint16_t id)
{
// name
if (id < PARAM_FIRST_UINT8) return (float)get((float_param_t)id);
else if (id < PARAM_FIRST_UINT16) return (float)get((uint8_param_t)id);
else if (id < PARAM_FIRST_INT16) return (float)get((uint16_param_t)id);
else if (id < PARAM_FIRST_UINT32) return (float)get((int16_param_t)id);
else if (id < PARAM_COUNT) return (float)get((uint32_param_t)id);
// XXX: uint32 won't have full bitwise precision
}
static const prog_char *getParamName(uint16_t id)
{
return (const prog_char *)pgm_read_word(&param_nametab[id]);
}
global_struct::global_struct() :
// parameters
// note, all values not explicitly initialised here are zeroed
param_count(PARAM_COUNT),
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second
// stream rates
streamRateRawSensors(1),
streamRateExtendedStatus(1),
streamRateRCChannels(1),
streamRateRawController(1),
streamRateRawSensorFusion(1),
streamRatePosition(1),
streamRateExtra1(1),
streamRateExtra2(1),
streamRateExtra3(1)
{
}
// Print
static void printParam(BetterStream & serial, uint16_t id)
{
serial.printf_P(PSTR("id %d %S, %f addr %d\n"), id, getParamName(id), getParamAsFloat(id), getAddress(id));
}
static void printAllParams(BetterStream & serial)
{
for (int i=0;i<global_data.param_count;i++)
printParam(serial,i);
}
static uint16_t getAddress(int id)
{
if (id < PARAM_FIRST_UINT8) return (id - PARAM_FIRST_FLOAT) * sizeof(float) + floatMemStart;
if (id < PARAM_FIRST_UINT16) return (id - PARAM_FIRST_UINT8) * sizeof(uint8_t) + uint8MemStart;
if (id < PARAM_FIRST_INT16) return (id - PARAM_FIRST_UINT16) * sizeof(uint16_t) + uint16MemStart;
if (id < PARAM_FIRST_UINT32) return (id - PARAM_FIRST_INT16) * sizeof(int16_t) + int16MemStart;
/*if (id < PARAM_COUNT)*/ return (id - PARAM_FIRST_UINT32) * sizeof(uint32_t) + uint32MemStart;
}
// Array interfaces
static uint8_t flight_mode(int i)
{
return get(uint8_param_t(PARAM_FLIGHT_MODE_1+i));
}
static void set_flight_mode(int i, uint8_t value)
{
set(uint8_param_t(PARAM_FLIGHT_MODE_1+i),value);
}
static uint16_t radio_min(int i)
{
return get(uint16_param_t(PARAM_RADIOMIN_CH1+i));
}
static void set_radio_min(int i, uint16_t value)
{
set(uint16_param_t(PARAM_RADIOMIN_CH1+i),value);
}
static uint16_t radio_max(int i)
{
return get(uint16_param_t(PARAM_RADIOMAX_CH1+i));
}
static void set_radio_max(int i, uint16_t value)
{
set(uint16_param_t(PARAM_RADIOMAX_CH1+i),value);
}
static uint16_t radio_trim(int i)
{
return get(uint16_param_t(PARAM_RADIOTRIM_CH1+i));
}
static void set_radio_trim(int i, uint16_t value)
{
set(uint16_param_t(PARAM_RADIOTRIM_CH1+i),value);
}

241
Tools/ArduTracker/navigation.pde
View File

@ -1,241 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//****************************************************************
// Function that will calculate the desired direction to fly and altitude error
//****************************************************************
void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (gps.fix == 0)
{
gps.new_data = false;
return;
}
if(next_WP.lat == 0){
return;
}
// We only perform most nav computations if we have new gps data to work with
// --------------------------------------------------------------------------
if(gps.new_data){
gps.new_data = false;
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrac correction
// ------------------------------------------
nav_bearing = target_bearing;
// waypoint distance from plane
// ----------------------------
wp_distance = getDistance(&current_loc, &next_WP);
if (wp_distance < 0){
gcs.send_text(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return;
}
// check if we have missed the WP
loiter_delta = (target_bearing - old_target_bearing)/100;
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta);
calc_bearing_error();
// control mode specific updates to nav_bearing
update_navigation();
}
}
void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
// distance and bearing calcs only
if(control_mode == AUTO){
verify_must();
verify_may();
}else{
switch(control_mode){
case LOITER:
update_loiter();
calc_bearing_error();
break;
case RTL:
if(wp_distance <= (get(PARAM_LOITER_RADIUS)) + LOITER_RANGE) {
set_mode(LOITER);
}else{
update_crosstrack();
}
break;
}
}
}
/*
Disabled for now
void calc_distance_error()
{
//distance_estimate += (float)gps.ground_speed * .0002f * cosf(radians(bearing_error * .01f));
//distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
//wp_distance = max(distance_estimate,10);
}
*/
void calc_airspeed_errors()
{
if(control_mode>=AUTO && airspeed_nudge > 0) {
airspeed_error = get(PARAM_TRIM_AIRSPEED) + airspeed_nudge - airspeed;
airspeed_energy_error = (long)(((long)(get(PARAM_TRIM_AIRSPEED) + airspeed_nudge) * (long)(get(PARAM_TRIM_AIRSPEED) + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
} else {
airspeed_error = get(PARAM_TRIM_AIRSPEED) - airspeed;
airspeed_energy_error = (long)(((long)get(PARAM_TRIM_AIRSPEED) * (long)get(PARAM_TRIM_AIRSPEED)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
}
}
void calc_bearing_error()
{
if(takeoff_complete == true || MAGNETOMETER == ENABLED) {
bearing_error = nav_bearing - dcm.yaw_sensor;
} else {
bearing_error = nav_bearing - gps.ground_course;
}
bearing_error = wrap_180(bearing_error);
}
void calc_altitude_error()
{
// limit climb rates
target_altitude = next_WP.alt - (long)(((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30)));
if(prev_WP.alt > next_WP.alt){
target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
}else{
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
}
/*
// Disabled for now
#if AIRSPEED_SENSOR == 1
// special thanks to Ryan Beall for this one
float pitch_angle = pitch_sensor - AOA; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°)
pitch_angle = constrain(pitch_angle, -2000, 2000);
float scale = sinf(radians(pitch_angle * .01));
altitude_estimate += (float)airspeed * .0002 * scale;
altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt);
// compute altitude error for throttle control
altitude_error = target_altitude - altitude_estimate;
#else
altitude_error = target_altitude - current_loc.alt;
#endif
*/
altitude_error = target_altitude - current_loc.alt;
}
long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
void update_loiter()
{
float power;
if (wp_distance <= get(PARAM_LOITER_RADIUS)){
power = float(wp_distance) / float(get(PARAM_LOITER_RADIUS));
nav_bearing += (int)(9000.0 * (2.0 + power));
} else if (wp_distance < (get(PARAM_LOITER_RADIUS) + LOITER_RANGE)){
power = -((float)(wp_distance - get(PARAM_LOITER_RADIUS) - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
} else {
update_crosstrack();
}
nav_bearing = wrap_360(nav_bearing);
/* loiter_delta = (target_bearing - old_target_bearing)/100;
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 170) loiter_delta -= 360;
if (loiter_delta < -170) loiter_delta += 360;
loiter_sum += loiter_delta;
*/
}
void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * get(PARAM_XTRACK_GAIN), -get(PARAM_XTRACK_ANGLE), get(PARAM_XTRACK_ANGLE));
nav_bearing = wrap_360(nav_bearing);
}
}
void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
long get_altitude_above_home(void)
{
// This is the altitude above the home location
// The GPS gives us altitude at Sea Level
// if you slope soar, you should see a negative number sometimes
// -------------------------------------------------------------
return current_loc.alt - home.alt;
}
long getDistance(struct Location *loc1, struct Location *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrtf(sq(dlat) + sq(dlong)) * .01113195f;
}
long get_alt_distance(struct Location *loc1, struct Location *loc2)
{
return abs(loc1->alt - loc2->alt);
}
long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f;
if (bearing < 0) bearing += 36000;
return bearing;
}

114
Tools/ArduTracker/param_init.pde
View File

@ -1,114 +0,0 @@
//
// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT
//
/// @file param_init.pde
void param_reset_defaults(void)
{
set(PARAM_HDNG2RLL_P, NAV_ROLL_P);
set(PARAM_HDNG2RLL_I, NAV_ROLL_I);
set(PARAM_HDNG2RLL_D, NAV_ROLL_D);
set(PARAM_HDNG2RLL_IMAX, NAV_ROLL_INT_MAX);
set(PARAM_RLL2SRV_P, SERVO_ROLL_P);
set(PARAM_RLL2SRV_I, SERVO_ROLL_I);
set(PARAM_RLL2SRV_D, SERVO_ROLL_D);
set(PARAM_RLL2SRV_IMAX, SERVO_ROLL_INT_MAX);
set(PARAM_PTCH2SRV_P, SERVO_PITCH_P);
set(PARAM_PTCH2SRV_I, SERVO_PITCH_I);
set(PARAM_PTCH2SRV_D, SERVO_PITCH_D);
set(PARAM_PTCH2SRV_IMAX, SERVO_PITCH_INT_MAX);
set(PARAM_ARSPD2PTCH_P, NAV_PITCH_ASP_P);
set(PARAM_ARSPD2PTCH_I, NAV_PITCH_ASP_I);
set(PARAM_ARSPD2PTCH_D, NAV_PITCH_ASP_D);
set(PARAM_ARSPD2PTCH_IMAX, NAV_PITCH_ASP_INT_MAX);
set(PARAM_YW2SRV_P, SERVO_YAW_P);
set(PARAM_YW2SRV_I, SERVO_YAW_I);
set(PARAM_YW2SRV_D, SERVO_YAW_D);
set(PARAM_YW2SRV_IMAX, SERVO_YAW_INT_MAX);
set(PARAM_ALT2THR_P, THROTTLE_ALT_P);
set(PARAM_ALT2THR_I, THROTTLE_ALT_I);
set(PARAM_ALT2THR_D, THROTTLE_ALT_D);
set(PARAM_ALT2THR_IMAX, THROTTLE_ALT_INT_MAX);
set(PARAM_ENRGY2THR_P, THROTTLE_TE_P);
set(PARAM_ENRGY2THR_I, THROTTLE_TE_I);
set(PARAM_ENRGY2THR_D, THROTTLE_TE_D);
set(PARAM_ENRGY2THR_IMAX, THROTTLE_TE_INT_MAX);
set(PARAM_ALT2PTCH_P, NAV_PITCH_ALT_P);
set(PARAM_ALT2PTCH_I, NAV_PITCH_ALT_I);
set(PARAM_ALT2PTCH_D, NAV_PITCH_ALT_D);
set(PARAM_ALT2PTCH_IMAX, NAV_PITCH_ALT_INT_MAX);
set(PARAM_KFF_PTCHCOMP, PITCH_COMP);
set(PARAM_KFF_RDDRMIX, RUDDER_MIX);
set(PARAM_KFF_PTCH2THR, P_TO_T);
set(PARAM_GND_ALT, 0);
set(PARAM_TRIM_AIRSPEED, AIRSPEED_CRUISE);
set(PARAM_XTRACK_ANGLE, XTRACK_ENTRY_ANGLE);
set(PARAM_LIM_ROLL, HEAD_MAX);
set(PARAM_LIM_PITCH_MAX, PITCH_MAX);
set(PARAM_LIM_PITCH_MIN, PITCH_MIN);
set(PARAM_ALT_MIX, ALTITUDE_MIX);
set(PARAM_ALT_HOLD_HOME, 0);
set(PARAM_ARSPD_RATIO, AIRSPEED_RATIO);
set(PARAM_IMU_OFFSET_0, 0);
set(PARAM_IMU_OFFSET_1, 0);
set(PARAM_IMU_OFFSET_2, 0);
set(PARAM_IMU_OFFSET_3, 0);
set(PARAM_IMU_OFFSET_4, 0);
set(PARAM_IMU_OFFSET_5, 0);
set(PARAM_YAW_MODE, 0);
set(PARAM_WP_MODE, 0);
set(PARAM_WP_TOTAL, WP_SIZE);
set(PARAM_WP_INDEX, 0);
set(PARAM_WP_RADIUS, WP_RADIUS_DEFAULT);
set(PARAM_LOITER_RADIUS, LOITER_RADIUS_DEFAULT);
set(PARAM_ARSPD_FBW_MIN, AIRSPEED_FBW_MIN);
set(PARAM_ARSPD_FBW_MAX, AIRSPEED_FBW_MAX);
set(PARAM_THR_MIN, THROTTLE_MIN);
set(PARAM_THR_MAX, THROTTLE_MAX);
set(PARAM_THR_FAILSAFE, THROTTLE_FAILSAFE);
set(PARAM_THR_FS_ACTION, THROTTLE_FAILSAFE_ACTION);
set(PARAM_TRIM_THROTTLE, THROTTLE_CRUISE);
set(PARAM_CONFIG, 0);
set(PARAM_TRIM_AUTO, AUTO_TRIM);
set(PARAM_SWITCH_ENABLE, REVERSE_SWITCH);
set(PARAM_FLIGHT_MODE_CH, FLIGHT_MODE_CHANNEL);
set(PARAM_FLIGHT_MODE_1, FLIGHT_MODE_1);
set(PARAM_FLIGHT_MODE_2, FLIGHT_MODE_2);
set(PARAM_FLIGHT_MODE_3, FLIGHT_MODE_3);
set(PARAM_FLIGHT_MODE_4, FLIGHT_MODE_4);
set(PARAM_FLIGHT_MODE_5, FLIGHT_MODE_5);
set(PARAM_FLIGHT_MODE_6, FLIGHT_MODE_6);
set(PARAM_FIRMWARE_VER, FIRMWARE_VERSION);
set(PARAM_RADIOTRIM_CH1, 1500);
set(PARAM_RADIOTRIM_CH2, 1500);
set(PARAM_RADIOTRIM_CH3, 1500);
set(PARAM_RADIOTRIM_CH4, 1500);
set(PARAM_RADIOTRIM_CH5, 1500);
set(PARAM_RADIOTRIM_CH6, 1500);
set(PARAM_RADIOTRIM_CH7, 1500);
set(PARAM_RADIOTRIM_CH8, 1500);
set(PARAM_RADIOMIN_CH1, 1000);
set(PARAM_RADIOMIN_CH2, 1000);
set(PARAM_RADIOMIN_CH3, 1000);
set(PARAM_RADIOMIN_CH4, 1000);
set(PARAM_RADIOMIN_CH5, CH5_MIN);
set(PARAM_RADIOMIN_CH6, CH6_MIN);
set(PARAM_RADIOMIN_CH7, CH7_MIN);
set(PARAM_RADIOMIN_CH8, CH8_MIN);
set(PARAM_RADIOMAX_CH1, 2000);
set(PARAM_RADIOMAX_CH2, 2000);
set(PARAM_RADIOMAX_CH3, 2000);
set(PARAM_RADIOMAX_CH4, 2000);
set(PARAM_RADIOMAX_CH5, CH5_MAX);
set(PARAM_RADIOMAX_CH6, CH6_MAX);
set(PARAM_RADIOMAX_CH7, CH7_MAX);
set(PARAM_RADIOMAX_CH8, CH8_MAX);
set(PARAM_LOG_BITMASK, 0);
set(PARAM_TRIM_ELEVON, 1500);
set(PARAM_THR_FS_VALUE, THROTTLE_FS_VALUE);
set(PARAM_XTRACK_GAIN, XTRACK_GAIN);
set(PARAM_GND_TEMP, 0);
set(PARAM_AP_OFFSET, 0);
set(PARAM_TRIM_PITCH, 0);
set(PARAM_GND_ABS_PRESS, 0);
}

230
Tools/ArduTracker/param_table.c
View File

@ -1,230 +0,0 @@
//
// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT
//
/// @file param_table.c
#pragma pack(push)
#pragma pack(1)
#include <stdint.h>
#include <avr/pgmspace.h>
#include "global_data.h"
#define PARAM_NAME(_name) static const char _param_ ## _name [] PROGMEM = #_name;
#define PARAM_INDEX(_name) [PARAM_ ## _name] = _param_ ## _name
PARAM_NAME(THR_FS_VALUE);
PARAM_NAME(XTRACK_GAIN);
PARAM_NAME(GND_TEMP);
PARAM_NAME(AP_OFFSET);
PARAM_NAME(TRIM_PITCH);
PARAM_NAME(GND_ABS_PRESS);
PARAM_NAME(PTCH2SRV_D);
PARAM_NAME(PTCH2SRV_IMAX);
PARAM_NAME(ARSPD2PTCH_P);
PARAM_NAME(ALT2THR_P);
PARAM_NAME(ARSPD2PTCH_I);
PARAM_NAME(ALT2THR_I);
PARAM_NAME(ARSPD2PTCH_D);
PARAM_NAME(ALT2THR_D);
PARAM_NAME(ARSPD2PTCH_IMAX);
PARAM_NAME(ALT2PTCH_D);
PARAM_NAME(ALT2THR_IMAX);
PARAM_NAME(YW2SRV_P);
PARAM_NAME(ALT2PTCH_IMAX);
PARAM_NAME(ENRGY2THR_P);
PARAM_NAME(YW2SRV_I);
PARAM_NAME(KFF_PTCHCOMP);
PARAM_NAME(ENRGY2THR_I);
PARAM_NAME(YW2SRV_D);
PARAM_NAME(LIM_PITCH_MIN);
PARAM_NAME(KFF_RDDRMIX);
PARAM_NAME(ENRGY2THR_D);
PARAM_NAME(YW2SRV_IMAX);
PARAM_NAME(ALT_MIX);
PARAM_NAME(KFF_PTCH2THR);
PARAM_NAME(ENRGY2THR_IMAX);
PARAM_NAME(ALT_HOLD_HOME);
PARAM_NAME(GND_ALT);
PARAM_NAME(ALT2PTCH_P);
PARAM_NAME(YAW_MODE);
PARAM_NAME(ARSPD_RATIO);
PARAM_NAME(TRIM_AIRSPEED);
PARAM_NAME(ALT2PTCH_I);
PARAM_NAME(WP_MODE);
PARAM_NAME(IMU_OFFSET_0);
PARAM_NAME(XTRACK_ANGLE);
PARAM_NAME(WP_TOTAL);
PARAM_NAME(IMU_OFFSET_1);
PARAM_NAME(LIM_ROLL);
PARAM_NAME(THR_FAILSAFE);
PARAM_NAME(WP_INDEX);
PARAM_NAME(IMU_OFFSET_2);
PARAM_NAME(LIM_PITCH_MAX);
PARAM_NAME(THR_FS_ACTION);
PARAM_NAME(WP_RADIUS);
PARAM_NAME(IMU_OFFSET_3);
PARAM_NAME(TRIM_THROTTLE);
PARAM_NAME(LOITER_RADIUS);
PARAM_NAME(IMU_OFFSET_4);
PARAM_NAME(CONFIG);
PARAM_NAME(ARSPD_FBW_MIN);
PARAM_NAME(IMU_OFFSET_5);
PARAM_NAME(FLIGHT_MODE_4);
PARAM_NAME(ARSPD_FBW_MAX);
PARAM_NAME(FLIGHT_MODE_5);
PARAM_NAME(TRIM_AUTO);
PARAM_NAME(THR_MIN);
PARAM_NAME(FLIGHT_MODE_6);
PARAM_NAME(SWITCH_ENABLE);
PARAM_NAME(THR_MAX);
PARAM_NAME(RADIOTRIM_CH7);
PARAM_NAME(FIRMWARE_VER);
PARAM_NAME(FLIGHT_MODE_CH);
PARAM_NAME(RADIOTRIM_CH8);
PARAM_NAME(RADIOTRIM_CH1);
PARAM_NAME(FLIGHT_MODE_1);
PARAM_NAME(RADIOMIN_CH1);
PARAM_NAME(RADIOTRIM_CH2);
PARAM_NAME(FLIGHT_MODE_2);
PARAM_NAME(RADIOMAX_CH1);
PARAM_NAME(RADIOMIN_CH2);
PARAM_NAME(RADIOTRIM_CH3);
PARAM_NAME(FLIGHT_MODE_3);
PARAM_NAME(RADIOMAX_CH2);
PARAM_NAME(RADIOMIN_CH3);
PARAM_NAME(RADIOTRIM_CH4);
PARAM_NAME(RADIOMAX_CH3);
PARAM_NAME(RADIOMIN_CH4);
PARAM_NAME(RADIOTRIM_CH5);
PARAM_NAME(RADIOMAX_CH4);
PARAM_NAME(RADIOMIN_CH5);
PARAM_NAME(RADIOTRIM_CH6);
PARAM_NAME(RADIOMAX_CH5);
PARAM_NAME(RADIOMIN_CH6);
PARAM_NAME(RADIOMAX_CH6);
PARAM_NAME(RADIOMIN_CH7);
PARAM_NAME(RADIOMAX_CH7);
PARAM_NAME(RADIOMIN_CH8);
PARAM_NAME(RADIOMAX_CH8);
PARAM_NAME(LOG_BITMASK);
PARAM_NAME(TRIM_ELEVON);
PARAM_NAME(HDNG2RLL_P);
PARAM_NAME(HDNG2RLL_I);
PARAM_NAME(HDNG2RLL_D);
PARAM_NAME(HDNG2RLL_IMAX);
PARAM_NAME(RLL2SRV_P);
PARAM_NAME(RLL2SRV_I);
PARAM_NAME(RLL2SRV_D);
PARAM_NAME(RLL2SRV_IMAX);
PARAM_NAME(PTCH2SRV_P);
PARAM_NAME(PTCH2SRV_I);
const char *param_nametab[] PROGMEM = {
PARAM_INDEX(THR_FS_VALUE),
PARAM_INDEX(XTRACK_GAIN),
PARAM_INDEX(GND_TEMP),
PARAM_INDEX(AP_OFFSET),
PARAM_INDEX(TRIM_PITCH),
PARAM_INDEX(GND_ABS_PRESS),
PARAM_INDEX(PTCH2SRV_D),
PARAM_INDEX(PTCH2SRV_IMAX),
PARAM_INDEX(ARSPD2PTCH_P),
PARAM_INDEX(ARSPD2PTCH_I),
PARAM_INDEX(ALT2THR_P),
PARAM_INDEX(ARSPD2PTCH_D),
PARAM_INDEX(ALT2THR_I),
PARAM_INDEX(ARSPD2PTCH_IMAX),
PARAM_INDEX(ALT2THR_D),
PARAM_INDEX(YW2SRV_P),
PARAM_INDEX(ALT2THR_IMAX),
PARAM_INDEX(ALT2PTCH_D),
PARAM_INDEX(YW2SRV_I),
PARAM_INDEX(ENRGY2THR_P),
PARAM_INDEX(ALT2PTCH_IMAX),
PARAM_INDEX(YW2SRV_D),
PARAM_INDEX(ENRGY2THR_I),
PARAM_INDEX(KFF_PTCHCOMP),
PARAM_INDEX(YW2SRV_IMAX),
PARAM_INDEX(ENRGY2THR_D),
PARAM_INDEX(KFF_RDDRMIX),
PARAM_INDEX(LIM_PITCH_MIN),
PARAM_INDEX(ENRGY2THR_IMAX),
PARAM_INDEX(KFF_PTCH2THR),
PARAM_INDEX(ALT_MIX),
PARAM_INDEX(ALT2PTCH_P),
PARAM_INDEX(GND_ALT),
PARAM_INDEX(ALT_HOLD_HOME),
PARAM_INDEX(ALT2PTCH_I),
PARAM_INDEX(TRIM_AIRSPEED),
PARAM_INDEX(ARSPD_RATIO),
PARAM_INDEX(YAW_MODE),
PARAM_INDEX(XTRACK_ANGLE),
PARAM_INDEX(IMU_OFFSET_0),
PARAM_INDEX(WP_MODE),
PARAM_INDEX(LIM_ROLL),
PARAM_INDEX(IMU_OFFSET_1),
PARAM_INDEX(WP_TOTAL),
PARAM_INDEX(LIM_PITCH_MAX),
PARAM_INDEX(IMU_OFFSET_2),
PARAM_INDEX(WP_INDEX),
PARAM_INDEX(THR_FAILSAFE),
PARAM_INDEX(IMU_OFFSET_3),
PARAM_INDEX(WP_RADIUS),
PARAM_INDEX(THR_FS_ACTION),
PARAM_INDEX(IMU_OFFSET_4),
PARAM_INDEX(LOITER_RADIUS),
PARAM_INDEX(TRIM_THROTTLE),
PARAM_INDEX(FLIGHT_MODE_4),
PARAM_INDEX(IMU_OFFSET_5),
PARAM_INDEX(ARSPD_FBW_MIN),
PARAM_INDEX(CONFIG),
PARAM_INDEX(TRIM_AUTO),
PARAM_INDEX(FLIGHT_MODE_5),
PARAM_INDEX(ARSPD_FBW_MAX),
PARAM_INDEX(SWITCH_ENABLE),
PARAM_INDEX(FLIGHT_MODE_6),
PARAM_INDEX(THR_MIN),
PARAM_INDEX(FLIGHT_MODE_CH),
PARAM_INDEX(FIRMWARE_VER),
PARAM_INDEX(RADIOTRIM_CH7),
PARAM_INDEX(THR_MAX),
PARAM_INDEX(FLIGHT_MODE_1),
PARAM_INDEX(RADIOTRIM_CH1),
PARAM_INDEX(RADIOTRIM_CH8),
PARAM_INDEX(FLIGHT_MODE_2),
PARAM_INDEX(RADIOTRIM_CH2),
PARAM_INDEX(RADIOMIN_CH1),
PARAM_INDEX(FLIGHT_MODE_3),
PARAM_INDEX(RADIOTRIM_CH3),
PARAM_INDEX(RADIOMIN_CH2),
PARAM_INDEX(RADIOMAX_CH1),
PARAM_INDEX(RADIOTRIM_CH4),
PARAM_INDEX(RADIOMIN_CH3),
PARAM_INDEX(RADIOMAX_CH2),
PARAM_INDEX(RADIOTRIM_CH5),
PARAM_INDEX(RADIOMIN_CH4),
PARAM_INDEX(RADIOMAX_CH3),
PARAM_INDEX(RADIOTRIM_CH6),
PARAM_INDEX(RADIOMIN_CH5),
PARAM_INDEX(RADIOMAX_CH4),
PARAM_INDEX(RADIOMIN_CH6),
PARAM_INDEX(RADIOMAX_CH5),
PARAM_INDEX(RADIOMIN_CH7),
PARAM_INDEX(RADIOMAX_CH6),
PARAM_INDEX(RADIOMIN_CH8),
PARAM_INDEX(RADIOMAX_CH7),
PARAM_INDEX(RADIOMAX_CH8),
PARAM_INDEX(LOG_BITMASK),
PARAM_INDEX(TRIM_ELEVON),
PARAM_INDEX(HDNG2RLL_P),
PARAM_INDEX(HDNG2RLL_I),
PARAM_INDEX(HDNG2RLL_D),
PARAM_INDEX(HDNG2RLL_IMAX),
PARAM_INDEX(RLL2SRV_P),
PARAM_INDEX(RLL2SRV_I),
PARAM_INDEX(RLL2SRV_D),
PARAM_INDEX(RLL2SRV_IMAX),
PARAM_INDEX(PTCH2SRV_P),
PARAM_INDEX(PTCH2SRV_I),
};

142
Tools/ArduTracker/param_table.h
View File

@ -1,142 +0,0 @@
//
// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT
//
/// @file param_table.h
#define PARAM_FIRST_FLOAT 0
enum float_param_t {
PARAM_HDNG2RLL_P = PARAM_FIRST_FLOAT,
PARAM_HDNG2RLL_I,
PARAM_HDNG2RLL_D,
PARAM_HDNG2RLL_IMAX,
PARAM_RLL2SRV_P,
PARAM_RLL2SRV_I,
PARAM_RLL2SRV_D,
PARAM_RLL2SRV_IMAX,
PARAM_PTCH2SRV_P,
PARAM_PTCH2SRV_I,
PARAM_PTCH2SRV_D,
PARAM_PTCH2SRV_IMAX,
PARAM_ARSPD2PTCH_P,
PARAM_ARSPD2PTCH_I,
PARAM_ARSPD2PTCH_D,
PARAM_ARSPD2PTCH_IMAX,
PARAM_YW2SRV_P,
PARAM_YW2SRV_I,
PARAM_YW2SRV_D,
PARAM_YW2SRV_IMAX,
PARAM_ALT2THR_P,
PARAM_ALT2THR_I,
PARAM_ALT2THR_D,
PARAM_ALT2THR_IMAX,
PARAM_ENRGY2THR_P,
PARAM_ENRGY2THR_I,
PARAM_ENRGY2THR_D,
PARAM_ENRGY2THR_IMAX,
PARAM_ALT2PTCH_P,
PARAM_ALT2PTCH_I,
PARAM_ALT2PTCH_D,
PARAM_ALT2PTCH_IMAX,
PARAM_KFF_PTCHCOMP,
PARAM_KFF_RDDRMIX,
PARAM_KFF_PTCH2THR,
PARAM_GND_ALT,
PARAM_TRIM_AIRSPEED,
PARAM_XTRACK_ANGLE,
PARAM_LIM_ROLL,
PARAM_LIM_PITCH_MAX,
PARAM_LIM_PITCH_MIN,
PARAM_ALT_MIX,
PARAM_ALT_HOLD_HOME,
PARAM_ARSPD_RATIO,
PARAM_IMU_OFFSET_0,
PARAM_IMU_OFFSET_1,
PARAM_IMU_OFFSET_2,
PARAM_IMU_OFFSET_3,
PARAM_IMU_OFFSET_4,
PARAM_IMU_OFFSET_5,
PARAM_LAST_FLOAT
};
#define PARAM_FLOAT_COUNT (PARAM_LAST_FLOAT - PARAM_FIRST_FLOAT)
#define PARAM_FIRST_UINT8 PARAM_LAST_FLOAT
enum uint8_param_t {
PARAM_YAW_MODE = PARAM_FIRST_UINT8,
PARAM_WP_MODE,
PARAM_WP_TOTAL,
PARAM_WP_INDEX,
PARAM_WP_RADIUS,
PARAM_LOITER_RADIUS,
PARAM_ARSPD_FBW_MIN,
PARAM_ARSPD_FBW_MAX,
PARAM_THR_MIN,
PARAM_THR_MAX,
PARAM_THR_FAILSAFE,
PARAM_THR_FS_ACTION,
PARAM_TRIM_THROTTLE,
PARAM_CONFIG,
PARAM_TRIM_AUTO,
PARAM_SWITCH_ENABLE,
PARAM_FLIGHT_MODE_CH,
PARAM_FLIGHT_MODE_1,
PARAM_FLIGHT_MODE_2,
PARAM_FLIGHT_MODE_3,
PARAM_FLIGHT_MODE_4,
PARAM_FLIGHT_MODE_5,
PARAM_FLIGHT_MODE_6,
PARAM_LAST_UINT8
};
#define PARAM_UINT8_COUNT (PARAM_LAST_UINT8 - PARAM_FIRST_UINT8)
#define PARAM_FIRST_UINT16 PARAM_LAST_UINT8
enum uint16_param_t {
PARAM_FIRMWARE_VER = PARAM_FIRST_UINT16,
PARAM_RADIOTRIM_CH1,
PARAM_RADIOTRIM_CH2,
PARAM_RADIOTRIM_CH3,
PARAM_RADIOTRIM_CH4,
PARAM_RADIOTRIM_CH5,
PARAM_RADIOTRIM_CH6,
PARAM_RADIOTRIM_CH7,
PARAM_RADIOTRIM_CH8,
PARAM_RADIOMIN_CH1,
PARAM_RADIOMIN_CH2,
PARAM_RADIOMIN_CH3,
PARAM_RADIOMIN_CH4,
PARAM_RADIOMIN_CH5,
PARAM_RADIOMIN_CH6,
PARAM_RADIOMIN_CH7,
PARAM_RADIOMIN_CH8,
PARAM_RADIOMAX_CH1,
PARAM_RADIOMAX_CH2,
PARAM_RADIOMAX_CH3,
PARAM_RADIOMAX_CH4,
PARAM_RADIOMAX_CH5,
PARAM_RADIOMAX_CH6,
PARAM_RADIOMAX_CH7,
PARAM_RADIOMAX_CH8,
PARAM_LOG_BITMASK,
PARAM_TRIM_ELEVON,
PARAM_THR_FS_VALUE,
PARAM_LAST_UINT16
};
#define PARAM_UINT16_COUNT (PARAM_LAST_UINT16 - PARAM_FIRST_UINT16)
#define PARAM_FIRST_INT16 PARAM_LAST_UINT16
enum int16_param_t {
PARAM_XTRACK_GAIN = PARAM_FIRST_INT16,
PARAM_GND_TEMP,
PARAM_AP_OFFSET,
PARAM_TRIM_PITCH,
PARAM_LAST_INT16
};
#define PARAM_INT16_COUNT (PARAM_LAST_INT16 - PARAM_FIRST_INT16)
#define PARAM_FIRST_UINT32 PARAM_LAST_INT16
enum uint32_param_t {
PARAM_GND_ABS_PRESS = PARAM_FIRST_UINT32,
PARAM_LAST_UINT32
};
#define PARAM_UINT32_COUNT (PARAM_LAST_UINT32 - PARAM_FIRST_UINT32)
#define PARAM_COUNT PARAM_LAST_UINT32

126
Tools/ArduTracker/paramgen.awk
View File

@ -1,126 +0,0 @@
#
# Process the parameter specification and produce the parameter enumerations and
# name table.
#
# See paramgen.in for details of the input format
#
BEGIN {
paramIndex = 0
typeIndex = 0
firstInType = 0
printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") > "param_table.h"
printf("/// @file param_table.h\n\n") > "param_table.h"
printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") > "param_init.pde"
printf("/// @file param_init.pde\n\n") > "param_init.pde"
printf("void param_reset_defaults(void)\n") > "param_init.pde"
printf("{\n") > "param_init.pde"
printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") > "param_table.c"
printf("/// @file param_table.c\n\n") > "param_table.c"
printf("#pragma pack(push)\n") > "param_table.c"
printf("#pragma pack(1)\n\n") > "param_table.c"
printf("#include <stdint.h>\n") > "param_table.c"
printf("#include <avr/pgmspace.h>\n") > "param_table.c"
printf("#include \"global_data.h\"\n\n") > "param_table.c"
printf("#define PARAM_NAME(_name) static const char _param_ ## _name [] PROGMEM = #_name;\n") > "param_table.c"
printf("#define PARAM_INDEX(_name) [PARAM_ ## _name] = _param_ ## _name\n\n") > "param_table.c"
}
function END_ENUM() {
printf(" PARAM_LAST_%s\n", currentType) > "param_table.h"
printf("};\n") > "param_table.h"
printf("#define PARAM_%s_COUNT (PARAM_LAST_%s - PARAM_FIRST_%s)\n\n",
currentType, currentType, currentType) > "param_table.h"
}
#
# skip lines containing comments
#
$1=="#" { next }
#
# process a type section change
#
$1=="type" {
newType = toupper($2)
# if there's a type already open, close it
if (currentType == "") {
# first enum opens at index zero
printf("#define PARAM_FIRST_%s 0\n", newType) > "param_table.h"
} else {
# finalise the preceding enum
END_ENUM()
# chain the next enum's starting value off the previous
printf("#define PARAM_FIRST_%s PARAM_LAST_%s\n", newType, currentType) > "param_table.h"
}
printf("enum %s_param_t {\n", tolower(newType)) > "param_table.h"
currentType = newType
firstInType = 1
next
}
#
# process a parameter name
#
NF >= 1 {
paramName = $1
paramInitial = $2
# emit the parameter inside the enum for param_table.h
if (firstInType) {
printf(" PARAM_%s = PARAM_FIRST_%s,\n", paramName, currentType) > "param_table.h"
firstInType = 0
} else {
printf(" PARAM_%s,\n", paramName) > "param_table.h"
}
# emit the call to the initialiser for param_init.pde
if (paramInitial != "") {
printf(" set(PARAM_%s, %s);\n", paramName, paramInitial) > "param_init.pde"
}
# save name for param_table.c
paramNames[paramIndex] = paramName
paramIndex++
}
END {
#
# close out the current enum
#
END_ENUM()
printf("#define PARAM_COUNT PARAM_LAST_%s\n", currentType) > "param_table.h"
#
# close the initialiser function
#
printf("}\n") > "param_init.pde"
#
# Generate param_table.c
#
# emit the PARAM_NAME invocations
for (name in paramNames) {
printf("PARAM_NAME(%s);\n", paramNames[name]) > "param_table.c"
}
# emit the PARAM_INDEX array
printf("\nconst char *param_nametab[] PROGMEM = {\n") > "param_table.c"
for (name in paramNames) {
printf(" PARAM_INDEX(%s),\n", paramNames[name]) > "param_table.c"
}
printf("};\n") > "param_table.c"
}

257
Tools/ArduTracker/paramgen.in
View File

@ -1,257 +0,0 @@
#
# Parameter database specification
#
# This file describes the global parameters used by the ArduPilot Mega software.
# Definitions here are used to produce the param_table.h, param_table.c and
# param_init.pde files.
#
# Process this file with "awk -f paramgen.awk paramgen.in"
#
#
# Parameters are grouped by type, with the software expecting that the types
# will be presented in the order float, uint8, uint16, int16, uint32.
#
# The following line formats are recognised
#
# type <type-name>
#
# Announces the start of the section of the file conatining parameters
# of the type <type-name>.
#
# <parameter-name>
#
# Defines a parameter with the name <parameter-name>. This will result in
# the generation of an enumeration named PARAM_<parameter-name>, and the
# creation of an entry in the global param_nametab array indexed by the
# enumeration, pointing to a PROGMEM string containing <parameter-name>.
#
# <parameter-name> <default-value>
#
# As above, but additionally an entry will be added to the param_init()
# function that will reset the parameter to <default_value>.
#
#
# ------------------------------------------------------------------
#
# !!! CHANGE THE FIRMWARE VERSION IF YOU MODIFY THIS FILE !!!
#
# If the firmware version in ROM and the firmware version
# in this file do not match it is assumed that the EEProm is
# in an unknown state and a factory reset is forced.
# This is to prevent the user from reading EEProm values that
# have been remapped to different locations in memory.
#
# ======================================================================
type float
# ------------------------------------------------------------------
# Gains
# ------------------------------------------------------------------
# Roll Control
# heading error from commnd to roll command deviation from trim
# (bank to turn strategy)
HDNG2RLL_P NAV_ROLL_P
HDNG2RLL_I NAV_ROLL_I
HDNG2RLL_D NAV_ROLL_D
HDNG2RLL_IMAX NAV_ROLL_INT_MAX
# roll error from command to roll servo deviation from trim
# (tracks commanded bank angle)
RLL2SRV_P SERVO_ROLL_P
RLL2SRV_I SERVO_ROLL_I
RLL2SRV_D SERVO_ROLL_D
RLL2SRV_IMAX SERVO_ROLL_INT_MAX
# ------------------------------------------------------------------
# Pitch Control
# pitch error from command to pitch servo deviation from trim
# (front-side strategy)
PTCH2SRV_P SERVO_PITCH_P
PTCH2SRV_I SERVO_PITCH_I
PTCH2SRV_D SERVO_PITCH_D
PTCH2SRV_IMAX SERVO_PITCH_INT_MAX
# airspeed error from commnd to pitch servo deviation from trim
# (back-side strategy)
ARSPD2PTCH_P NAV_PITCH_ASP_P
ARSPD2PTCH_I NAV_PITCH_ASP_I
ARSPD2PTCH_D NAV_PITCH_ASP_D
ARSPD2PTCH_IMAX NAV_PITCH_ASP_INT_MAX
# ------------------------------------------------------------------
# Yaw Control
# yaw rate error from commnd to yaw servo deviation from trim
# (stabilizes dutch roll)
YW2SRV_P SERVO_YAW_P
YW2SRV_I SERVO_YAW_I
YW2SRV_D SERVO_YAW_D
YW2SRV_IMAX SERVO_YAW_INT_MAX
# ------------------------------------------------------------------
# Throttle Control
# altitude error from commnd to throttle servo deviation from trim
# (throttle back-side strategy)
ALT2THR_P THROTTLE_ALT_P
ALT2THR_I THROTTLE_ALT_I
ALT2THR_D THROTTLE_ALT_D
ALT2THR_IMAX THROTTLE_ALT_INT_MAX
# total energy error from commnd to throttle servo deviation from trim
# (throttle back-side strategy alternative)
ENRGY2THR_P THROTTLE_TE_P
ENRGY2THR_I THROTTLE_TE_I
ENRGY2THR_D THROTTLE_TE_D
ENRGY2THR_IMAX THROTTLE_TE_INT_MAX
# altitude error from commnd to pitch servo deviation from trim
# (throttle front-side strategy alternative)
ALT2PTCH_P NAV_PITCH_ALT_P
ALT2PTCH_I NAV_PITCH_ALT_I
ALT2PTCH_D NAV_PITCH_ALT_D
ALT2PTCH_IMAX NAV_PITCH_ALT_INT_MAX
# feed forward gains
KFF_PTCHCOMP PITCH_COMP
KFF_RDDRMIX RUDDER_MIX
KFF_PTCH2THR P_TO_T
# misc
GND_ALT 0
TRIM_AIRSPEED AIRSPEED_CRUISE
XTRACK_ANGLE XTRACK_ENTRY_ANGLE
# limits
LIM_ROLL HEAD_MAX
LIM_PITCH_MAX PITCH_MAX
LIM_PITCH_MIN PITCH_MIN
# estimation
ALT_MIX ALTITUDE_MIX
ALT_HOLD_HOME 0
ARSPD_RATIO AIRSPEED_RATIO
# ------------------------------------------------------------------
# IMU Calibration
IMU_OFFSET_0 0
IMU_OFFSET_1 0
IMU_OFFSET_2 0
IMU_OFFSET_3 0
IMU_OFFSET_4 0
IMU_OFFSET_5 0
# ======================================================================
type uint8
# not used currently
YAW_MODE 0
# waypoints
WP_MODE 0
WP_TOTAL WP_SIZE
WP_INDEX 0
WP_RADIUS WP_RADIUS_DEFAULT
LOITER_RADIUS LOITER_RADIUS_DEFAULT
# fly by wire
ARSPD_FBW_MIN AIRSPEED_FBW_MIN
ARSPD_FBW_MAX AIRSPEED_FBW_MAX
# throttle
THR_MIN THROTTLE_MIN
THR_MAX THROTTLE_MAX
THR_FAILSAFE THROTTLE_FAILSAFE
THR_FS_ACTION THROTTLE_FAILSAFE_ACTION
TRIM_THROTTLE THROTTLE_CRUISE
# misc
CONFIG 0
TRIM_AUTO AUTO_TRIM
SWITCH_ENABLE REVERSE_SWITCH
# flight modes
FLIGHT_MODE_CH FLIGHT_MODE_CHANNEL
FLIGHT_MODE_1 FLIGHT_MODE_1
FLIGHT_MODE_2 FLIGHT_MODE_2
FLIGHT_MODE_3 FLIGHT_MODE_3
FLIGHT_MODE_4 FLIGHT_MODE_4
FLIGHT_MODE_5 FLIGHT_MODE_5
FLIGHT_MODE_6 FLIGHT_MODE_6
# ======================================================================
type uint16
FIRMWARE_VER FIRMWARE_VERSION
# ------------------------------------------------------------------
# Radio Settings
#
# all radio settings are uint16_t and represent the period of the
# pulse width modulated signal. Typically 1000 ms is the lower limit,
# 1500 is neutral, and 2000 is the upper limit
# trim (neutral setting)
RADIOTRIM_CH1 1500
RADIOTRIM_CH2 1500
RADIOTRIM_CH3 1500
RADIOTRIM_CH4 1500
RADIOTRIM_CH5 1500
RADIOTRIM_CH6 1500
RADIOTRIM_CH7 1500
RADIOTRIM_CH8 1500
# min (maps to 0%)
RADIOMIN_CH1 1000
RADIOMIN_CH2 1000
RADIOMIN_CH3 1000
RADIOMIN_CH4 1000
RADIOMIN_CH5 CH5_MIN
RADIOMIN_CH6 CH6_MIN
RADIOMIN_CH7 CH7_MIN
RADIOMIN_CH8 CH8_MIN
# max (maps to 100%)
RADIOMAX_CH1 2000
RADIOMAX_CH2 2000
RADIOMAX_CH3 2000
RADIOMAX_CH4 2000
RADIOMAX_CH5 CH5_MAX
RADIOMAX_CH6 CH6_MAX
RADIOMAX_CH7 CH7_MAX
RADIOMAX_CH8 CH8_MAX
# ------------------------------------------------------------------
# Misc
LOG_BITMASK 0
TRIM_ELEVON 1500
THR_FS_VALUE THROTTLE_FS_VALUE
# ======================================================================
type int16
# ------------------------------------------------------------------
# Misc
XTRACK_GAIN XTRACK_GAIN
GND_TEMP 0
AP_OFFSET 0
TRIM_PITCH 0
# ======================================================================
type uint32
# ------------------------------------------------------------------
# Misc
GND_ABS_PRESS 0

199
Tools/ArduTracker/radio.pde
View File

@ -1,199 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
void read_radio()
{
ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH);
if(mix_mode == 0){
radio_in[CH_ROLL] = ch1_temp;
radio_in[CH_PITCH] = ch2_temp;
}else{
radio_in[CH_ROLL] = reverse_elevons * (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) - reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500;
radio_in[CH_PITCH] = (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) + reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500;
}
for (int y = 2; y < 8; y++)
radio_in[y] = APM_RC.InputCh(y);
#if THROTTLE_REVERSE == 1
radio_in[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_in[CH_THROTTLE];
#endif
throttle_failsafe(radio_in[CH_THROTTLE]);
servo_out[CH_THROTTLE] = ((float)(radio_in[CH_THROTTLE] - radio_min(CH_THROTTLE)) /
(float)(radio_max(CH_THROTTLE) - radio_min(CH_THROTTLE))) * 100;
servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100);
if (servo_out[CH_THROTTLE] > 50) {
if(AIRSPEED_SENSOR == 1) {
airspeed_nudge = (get(PARAM_ARSPD_FBW_MAX) - get(PARAM_TRIM_AIRSPEED)) * ((servo_out[CH_THROTTLE]-50) * 2);
} else {
throttle_nudge = (get(PARAM_THR_MAX) - get(PARAM_TRIM_THROTTLE)) * ((servo_out[CH_THROTTLE]-50) * 2);
}
} else {
airspeed_nudge = 0;
throttle_nudge = 0;
}
}
void throttle_failsafe(uint16_t pwm)
{
if(get(PARAM_THR_FAILSAFE)== 0)
return;
//check for failsafe and debounce funky reads
// ------------------------------------------
if (pwm < get(PARAM_THR_FS_VALUE)){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9){
SendDebug("MSG FS ON ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 10) {
ch3_failsafe = true;
//set_failsafe(true);
//failsafeCounter = 10;
}else if (failsafeCounter > 10){
failsafeCounter = 11;
}
}else if(failsafeCounter > 0){
// we are no longer in failsafe condition
// but we need to recover quickly
failsafeCounter--;
if (failsafeCounter > 3){
failsafeCounter = 3;
}
if (failsafeCounter == 1){
SendDebug("MSG FS OFF ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 0) {
ch3_failsafe = false;
//set_failsafe(false);
//failsafeCounter = -1;
}else if (failsafeCounter <0){
failsafeCounter = -1;
}
}
}
void trim_control_surfaces()
{
// Store control surface trim values
// ---------------------------------
if(mix_mode == 0){
set_radio_trim(CH_ROLL,radio_in[CH_ROLL]);
set_radio_trim(CH_PITCH,radio_in[CH_PITCH]);
set_radio_trim(CH_RUDDER,radio_in[CH_RUDDER]);
}else{
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
//Recompute values here using new values for elevon1_trim and elevon2_trim
//We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed
uint16_t center = 1500;
set_radio_trim(CH_ROLL,center);
set_radio_trim(CH_PITCH,center);
}
// disabled for now
//save_EEPROM_trims();
}
void trim_radio()
{
for (int y = 0; y < 50; y++) {
read_radio();
}
// Store the trim values
// ---------------------
if(mix_mode == 0){
for (int y = 0; y < 8; y++) set_radio_trim(y,radio_in[y]);
}else{
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
uint16_t center = 1500;
set_radio_trim(CH_ROLL,center);
set_radio_trim(CH_PITCH,center);
for (int y = 2; y < 8; y++) set_radio_trim(y,radio_in[y]);
}
//save_EEPROM_trims();
}
#if SET_RADIO_LIMITS == 1
void read_radio_limits()
{
// set initial servo limits for calibration routine
// -------------------------------------------------
set_radio_min(CH_ROLL,radio_in[CH_ROLL] - 150);
set_radio_max(CH_ROLL,radio_in[CH_ROLL] + 150);
set_radio_min(CH_PITCH,radio_in[CH_PITCH] - 150);
set_radio_max(CH_PITCH,radio_in[CH_PITCH] + 150);
// vars for the radio config routine
// ---------------------------------
int counter = 0;
long reminder;
reminder = millis() - 10000;
uint16_t tempMin[2] = {radio_min(CH_ROLL), radio_min(CH_PITCH)} ;
uint16_t tempMax[2] = {radio_max(CH_ROLL), radio_max(CH_PITCH)} ;
// Allows user to set stick limits and calibrate the IR
// ----------------------------------------------------
while(counter < 50){
if (millis() - reminder >= 10000) { // Remind user every 10 seconds what is going on
GCS.send_text(SEVERITY_LOW,"Reading radio limits:");
GCS.send_text(SEVERITY_LOW,"Move sticks to: upper right and lower Left");
GCS.send_text(SEVERITY_LOW,"To Continue, hold the stick in the corner for 2 seconds.");
GCS.send_text(SEVERITY_LOW," ");
//print_radio();
demo_servos(1);
reminder = millis();
}
delay(40);
read_radio();
// AutoSet servo limits
// --------------------
if (radio_in[CH_ROLL] > 1000 && radio_in[CH_ROLL] < 2000){
tempMin[CH_ROLL] = min(radio_in[CH_ROLL], tempMin[CH_ROLL]);
tempMax[CH_ROLL] = max(radio_in[CH_ROLL], tempMax[CH_ROLL]);
}
if (radio_in[CH_PITCH] > 1000 && radio_in[CH_PITCH]< 2000){
tempMin[CH_PITCH] = min(radio_in[CH_PITCH], tempMin[CH_PITCH]);
tempMax[CH_PITCH] = max(radio_in[CH_PITCH], tempMax[CH_PITCH]);
}
if(radio_in[CH_PITCH] < (tempMin[CH_PITCH] + 30) || tempMin[CH_PITCH] > (tempMax[CH_PITCH] -30)){
SendDebug(".");
counter++;
}else{
if (counter > 0)
counter--;
}
}
// contstrain min values
// ---------------------
set_radio_min(CH_ROLL,constrain(tempMin[CH_ROLL], 800, 2200));
set_radio_max(CH_ROLL,constrain(tempMax[CH_ROLL], 800, 2200));
set_radio_min(CH_PITCH,constrain(tempMin[CH_PITCH], 800, 2200));
set_radio_max(CH_PITCH,constrain(tempMax[CH_PITCH], 800, 2200));
SendDebugln(" ");
}
#endif

70
Tools/ArduTracker/sensors.pde
View File

@ -1,70 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
void ReadSCP1000(void) {}
// Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
void read_airpressure(void){
double x;
pitot.Read(); // Get new data from absolute pressure sensor
abs_press = pitot.Press;
abs_press_filt = (abs_press); // + 2l * abs_press_filt) / 3l; // Light filtering
//temperature = (temperature * 9 + temp_unfilt) / 10; We will just use the ground temp for the altitude calculation
double p = (double)abs_press_gnd / (double)abs_press_filt;
double temp = (float)ground_temperature / 10.f + 273.15f;
x = log(p) * temp * 29271.267f;
//x = log(p) * temp * 29.271267 * 1000;
press_alt = (long)(x / 10) + ground_alt; // Pressure altitude in centimeters
// Need to add comments for theory.....
}
// in M/S * 100
void read_airspeed(void)
{
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed
airpressure_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airpressure_raw * .75);
airpressure = (int)airpressure_raw - airpressure_offset;
airpressure = max(airpressure, 0);
airspeed = sqrtf((float)airpressure * get(PARAM_ARSPD_RATIO)) * 100;
#endif
calc_airspeed_errors();
}
void zero_airspeed(void)
{
airpressure_raw = (float)adc.Ch(AIRSPEED_CH);
for(int c = 0; c < 50; c++){
delay(20);
airpressure_raw = (airpressure_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10);
}
airpressure_offset = airpressure_raw;
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
#if BATTERY_EVENT == 1
void read_battery(void)
{
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
#if BATTERY_TYPE == 0
if(battery_voltage3 < LOW_VOLTAGE)
low_battery_event();
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
#endif
#if BATTERY_TYPE == 1
if(battery_voltage4 < LOW_VOLTAGE)
low_battery_event();
battery_voltage = battery_voltage4; // set total battery voltage, for telemetry stream
#endif
}
#endif

229
Tools/ArduTracker/setup.pde
View File

@ -1,229 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Functions called from the setup menu
static int8_t setup_radio(uint8_t argc, const Menu::arg *argv);
static int8_t setup_show(uint8_t argc, const Menu::arg *argv);
static int8_t setup_factory(uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv);
// Command/function table for the setup menu
const struct Menu::command setup_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"reset", setup_factory},
{"radio", setup_radio},
{"modes", setup_flightmodes},
{"show", setup_show},
};
// Create the setup menu object.
MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu.
int8_t
setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
Serial.printf_P(PSTR("Setup Mode\n"
"\n"
"IMPORTANT: if you have not previously set this system up, use the\n"
"'reset' command to initialize the EEPROM to sensible default values\n"
"and then the 'radio' command to configure for your radio.\n"
"\n"));
// Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run();
}
// Print the current configuration.
// Called by the setup menu 'show' command.
static int8_t
setup_show(uint8_t argc, const Menu::arg *argv)
{
printAllParams(Serial);
return(0);
}
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
// Called by the setup menu 'factoryreset' command.
static int8_t
setup_factory(uint8_t argc, const Menu::arg *argv)
{
uint8_t i;
int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
Serial.printf_P(PSTR("\nFACTORY RESET\n\n"));
param_reset_defaults();
return(0);
}
// Perform radio setup.
// Called by the setup menu 'radio' command.
static int8_t
setup_radio(uint8_t argc, const Menu::arg *argv)
{
Serial.println("\n\nRadio Setup:");
uint8_t i;
for(i = 0; i < 100;i++){
delay(20);
read_radio();
}
if(radio_in[CH_ROLL] < 500){
while(1){
Serial.print("Radio error");
delay(1000);
// stop here
}
}
uint16_t tempMin[4], tempMax[4];
for(i = 0; i < 4; i++){
tempMin[i] = radio_in[i];
tempMax[i] = radio_in[i];
}
Serial.printf_P(PSTR("\nMove both sticks to each corner. Hit Enter to save: "));
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
for(i = 0; i < 4; i++){
tempMin[i] = min(tempMin[i], radio_in[i]);
tempMax[i] = max(tempMax[i], radio_in[i]);
}
if(Serial.available() > 0){
Serial.flush();
Serial.println("Saving:");
for(i = 0; i < 4; i++)
{
set_radio_max(i, tempMax[i]);
set_radio_min(i, tempMin[i]);
}
for(i = 0; i < 8; i++)
Serial.printf_P(PSTR("CH%d: %d | %d\n"), i + 1, radio_min(i), radio_max(i));
break;
}
}
return(0);
}
static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
byte switchPosition, oldSwitchPosition, mode;
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter();
trim_radio();
while(1){
delay(20);
read_radio();
switchPosition = readSwitch();
// look for control switch change
if (oldSwitchPosition != switchPosition){
// Override position 5
if(switchPosition > 4){
set_flight_mode(switchPosition,MANUAL);
mode = MANUAL;
}else{
// update our current mode
mode = flight_mode(switchPosition);
}
// update the user
print_switch(switchPosition, mode);
// Remember switch position
oldSwitchPosition = switchPosition;
}
// look for stick input
int radioInputSwitch = radio_input_switch();
if (radioInputSwitch != 0){
mode += radioInputSwitch;
while (
mode != MANUAL &&
mode != CIRCLE &&
mode != STABILIZE &&
mode != FLY_BY_WIRE_A &&
mode != FLY_BY_WIRE_B &&
mode != AUTO &&
mode != RTL &&
mode != LOITER &&
mode != TAKEOFF &&
mode != LAND)
{
if (mode < MANUAL)
mode = LAND;
else if (mode >LAND)
mode = MANUAL;
else
mode += radioInputSwitch;
}
// Override position 5
if(switchPosition > 4)
mode = MANUAL;
// save new mode
set_flight_mode(switchPosition,mode);
// print new mode
print_switch(switchPosition, mode);
}
// escape hatch
if(Serial.available() > 0){
//save_EEPROM_flight_modes();
return (0);
}
}
}
void
print_switch(byte p, byte m)
{
Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]);
}
int8_t
radio_input_switch(void)
{
static int8_t bouncer = 0;
if (int16_t(radio_in[0] - radio_trim(0)) > 200) bouncer = 10;
if (int16_t(radio_in[0] - radio_trim(0)) < -200) bouncer = -10;
if (bouncer >0) bouncer --;
if (bouncer <0) bouncer ++;
if (bouncer == 1 || bouncer == -1) return bouncer;
else return 0;
}

511
Tools/ArduTracker/system.pde
View File

@ -1,511 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*****************************************************************************
The init_ardupilot function processes everything we need for an in - air restart
We will determine later if we are actually on the ground and process a
ground start in that case.
*****************************************************************************/
// Functions called from the top-level menu
extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads from flash memory
static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Commands:\n"
" logs log readback/setup mode\n"
" setup setup mode\n"
" test test mode\n"
"\n"
"Move the slide switch and reset to FLY.\n"
"\n"));
return(0);
}
// Command/function table for the top-level menu.
const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"logs", process_logs},
{"setup", setup_mode},
{"test", test_mode},
{"help", main_menu_help}
};
// Create the top-level menu object.
MENU(main_menu, "ArduPilotMega", main_menu_commands);
void init_ardupilot()
{
byte last_log_num;
int last_log_start;
int last_log_end;
// Console serial port
//
// The console port buffers are defined to be sufficiently large to support
// the console's use as a logging device, optionally as the GPS port when
// GPS_PROTOCOL_IMU is selected, and as the telemetry port.
//
// XXX This could be optimised to reduce the buffer sizes in the cases
// where they are not otherwise required.
//
Serial.begin(SERIAL0_BAUD, 128, 128);
// GPS serial port.
//
// Not used if the IMU/X-Plane GPS is in use.
//
// XXX currently the EM406 (SiRF receiver) is nominally configured
// at 57600, however it's not been supported to date. We should
// probably standardise on 38400.
//
// XXX the 128 byte receive buffer may be too small for NMEA, depending
// on the message set configured.
//
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT == 1 // TODO: figure out a better way to do this
// Steal gps port for hardware in the loop
Serial1.begin(115200, 128, 128);
#else
// standard gps running
Serial1.begin(38400, 128, 16);
#endif
// Telemetry port.
//
// Not used if telemetry is going to the console.
//
// XXX for unidirectional protocols, we could (should) minimize
// the receive buffer, and the transmit buffer could also be
// shrunk for protocols that don't send large messages.
//
Serial3.begin(SERIAL3_BAUD, 128, 128);
Serial.printf_P(PSTR("\n\n"
"Init ArduPilotMega (unstable development version)\n\n"
"Firmware Version: %d freeRAM: %lu\n"),
FIRMWARE_VERSION, freeRAM());
// !!! Check firmware version before loading any
// data from EEPROM !!!
if (FIRMWARE_VERSION != get(PARAM_FIRMWARE_VER))
{
Serial.printf_P(PSTR("\n\n"
"Firmware Mismatch! ROM Firmware Version: %d\n"
"\nA factory reset is being performed."),get(PARAM_FIRMWARE_VER));
// If firmware mismatches a factory reset is forced
param_reset_defaults();
}
APM_RC.Init(); // APM Radio initialization
//read_EEPROM_startup(); // Read critical config information to start
#if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization
pitot.Init(); // APM Abs Pressure sensor initialization
compass.init(); // I2C initialization
#endif
DataFlash.Init(); // DataFlash log initialization
gps.init(); // GPS Initialization
// init the GCS
#if GCS_PORT == 3
gcs.init(&Serial3);
#else
gcs.init(&Serial);
#endif
// init the HIL
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PORT == 3
hil.init(&Serial3);
#elif HIL_PORT == 1
hil.init(&Serial1);
#else
hil.init(&Serial);
#endif
#endif
APM_RC.OutputCh(CH_ROLL, radio_trim(CH_ROLL)); // Initialization of servo outputs
APM_RC.OutputCh(CH_PITCH, radio_trim(CH_PITCH));
APM_RC.OutputCh(CH_THROTTLE, radio_trim(CH_THROTTLE));
APM_RC.OutputCh(CH_RUDDER, radio_trim(CH_RUDDER));
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode
pinMode(PUSHBUTTON_PIN, INPUT); // unused
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
// If the switch is in 'menu' mode, run the main menu.
//
// Since we can't be sure that the setup or test mode won't leave
// the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly.
//
if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
Serial.printf_P(PSTR("\n"
"Entering interactive setup mode...\n"
"\n"
"If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n"
"Type 'help' to list commands, 'exit' to leave a submenu.\n"
"Visit the 'setup' menu for first-time configuration.\n"));
for (;;) {
Serial.printf_P(PSTR("\n"
"Move the slide switch and reset to FLY.\n"
"\n"));
main_menu.run();
}
}
if(get(PARAM_LOG_BITMASK) > 0){
// Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off
// XXX: TODO implement for new struct
//last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM);
//last_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(last_log_num - 1) * 0x02));
//last_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE);
if(last_log_num == 0) {
// The log space is empty. Start a write session on page 1
DataFlash.StartWrite(1);
eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (1));
eeprom_write_word((uint16_t *) EE_LOG_1_START, (1));
} else if (last_log_end <= last_log_start + 10) {
// The last log is small. We consider it junk. Overwrite it.
DataFlash.StartWrite(last_log_start);
} else {
// The last log is valid. Start a new log
if(last_log_num >= 19) {
Serial.println("Number of log files exceeds max. Log 19 will be overwritten.");
last_log_num --;
}
DataFlash.StartWrite(last_log_end + 1);
eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (last_log_num + 1));
eeprom_write_word((uint16_t *) (EE_LOG_1_START+(last_log_num)*0x02), (last_log_end + 1));
}
}
// read in the flight switches
update_servo_switches();
if(DEBUG_SUBSYSTEM > 0){
debug_subsystem();
} else if (ENABLE_AIR_START == 1) {
// Perform an air start and get back to flying
gcs.send_text(SEVERITY_LOW,"<init_ardupilot> AIR START");
// Get necessary data from EEPROM
//----------------
//read_EEPROM_airstart_critical();
#if HIL_MODE != HIL_MODE_ATTITUDE
imu.load_gyro_eeprom();
imu.load_accel_eeprom();
#endif
// This delay is important for the APM_RC library to work.
// We need some time for the comm between the 328 and 1280 to be established.
int old_pulse = 0;
while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH))) > 5 ||
APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH)) == 1000 ||
APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH)) == 1200)) {
old_pulse = APM_RC.InputCh(get(PARAM_FLIGHT_MODE_CH));
delay(25);
}
if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD)
Log_Write_Startup(TYPE_AIRSTART_MSG);
reload_commands(); // Get set to resume AUTO from where we left off
}else {
startup_ground();
if (get(PARAM_LOG_BITMASK) & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
}
// set the correct flight mode
// ---------------------------
reset_control_switch();
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
void startup_ground(void)
{
gcs.send_text(SEVERITY_LOW,"<startup_ground> GROUND START");
#if(GROUND_START_DELAY > 0)
gcs.send_text(SEVERITY_LOW,"<startup_ground> With Delay");
delay(GROUND_START_DELAY * 1000);
#endif
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < get(PARAM_WP_TOTAL) + 1; i++) {
gcs.send_message(MSG_COMMAND_LIST, i);
}
// Makes the servos wiggle
// step 1 = 1 wiggle
// -----------------------
demo_servos(1);
//IMU ground start
//------------------------
//
startup_IMU_ground();
// read the radio to set trims
// ---------------------------
trim_radio();
#if HIL_MODE != HIL_MODE_ATTITUDE
# if AIRSPEED_SENSOR == ENABLED
// initialize airspeed sensor
// --------------------------
zero_airspeed();
gcs.send_text(SEVERITY_LOW,"<startup_ground> zero airspeed calibrated");
# else
gcs.send_text(SEVERITY_LOW,"<startup_ground> NO airspeed");
# endif
#endif
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
// initialize commands
// -------------------
init_commands();
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
demo_servos(3);
gcs.send_text(SEVERITY_LOW,"\n\n Ready to FLY.");
}
void set_mode(byte mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
if(get(PARAM_TRIM_AUTO) > 0 || control_mode == MANUAL)
trim_control_surfaces();
control_mode = mode;
crash_timer = 0;
switch(control_mode)
{
case MANUAL:
break;
case STABILIZE:
break;
case FLY_BY_WIRE_A:
break;
case FLY_BY_WIRE_B:
break;
case AUTO:
update_auto();
break;
case RTL:
return_to_launch();
break;
case LOITER:
loiter_at_location();
break;
case TAKEOFF:
break;
case LAND:
break;
default:
return_to_launch();
break;
}
// output control mode to the ground station
gcs.send_message(MSG_MODE_CHANGE);
if (get(PARAM_LOG_BITMASK) & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
void set_failsafe(boolean mode)
{
// only act on changes
// -------------------
if(failsafe != mode){
// store the value so we don't trip the gate twice
// -----------------------------------------------
failsafe = mode;
if (failsafe == false){
// We're back in radio contact
// ---------------------------
// re-read the switch so we can return to our preferred mode
reset_control_switch();
// Reset control integrators
// ---------------------
reset_I();
}else{
// We've lost radio contact
// ------------------------
// nothing to do right now
}
// Let the user know what's up so they can override the behavior
// -------------------------------------------------------------
failsafe_event();
}
}
void startup_IMU_ground(void)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
uint16_t store = 0;
int flashcount = 0;
SendDebugln("<startup_IMU_ground> Warming up ADC...");
delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
// -----------------------
demo_servos(2);
SendDebugln("<startup_IMU_ground> Beginning IMU calibration; do not move plane");
delay(1000);
imu.init_accel();
imu.init_gyro();
# if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets for initialization
# endif
pitot.Read(); // Get initial data from absolute pressure sensor
abs_press_gnd = pitot.Press;
ground_temperature = pitot.Temp;
delay(20);
// ***********
for(int i = 0; i < 200; i++){ // We take some readings...
# if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets
# endif
pitot.Read(); // Get initial data from absolute pressure sensor
abs_press_gnd = (abs_press_gnd * 9l + pitot.Press) / 10l;
ground_temperature = (ground_temperature * 9 + pitot.Temp) / 10;
delay(20);
if(flashcount == 5) {
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, LOW);
}
if(flashcount >= 10) {
flashcount = 0;
digitalWrite(C_LED_PIN, HIGH);
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, HIGH);
}
flashcount++;
}
SendDebugln(" <startup_IMU_ground> Calibration complete.");
#endif // HIL_MODE_ATTITUDE
digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW);
}
void update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
switch (gps.status()) {
case(2):
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
break;
case(1):
if (gps.valid_read == true){
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (GPS_light){
digitalWrite(C_LED_PIN, LOW);
} else {
digitalWrite(C_LED_PIN, HIGH);
}
gps.valid_read = false;
}
break;
default:
digitalWrite(C_LED_PIN, LOW);
break;
}
}
void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
gyro_sat_count = 0;
adc_constraints = 0;
renorm_sqrt_count = 0;
renorm_blowup_count = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
}
/*
* This function gets the current value of the heap and stack pointers.
* The stack pointer starts at the top of RAM and grows downwards. The heap pointer
* starts just above the static variables etc. and grows upwards. SP should always
* be larger than HP or you'll be in big trouble! The smaller the gap, the more
* careful you need to be. Julian Gall 6 - Feb - 2009.
*/
unsigned long freeRAM() {
uint8_t * heapptr, * stackptr;
stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
heapptr = stackptr; // save value of heap pointer
free(stackptr); // free up the memory again (sets stackptr to 0)
stackptr = (uint8_t *)(SP); // save value of stack pointer
return stackptr - heapptr;
}

520
Tools/ArduTracker/test.pde
View File

@ -1,520 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
static int8_t test_gyro(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
const struct Menu::command test_menu_commands[] PROGMEM = {
{"radio", test_radio},
{"battery", test_battery},
{"relay", test_relay},
{"waypoints", test_wp},
{"xbee", test_xbee},
{"eedump", test_eedump},
{"modeswitch", test_modeswitch},
{"dipswitches", test_dipswitches},
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
#if HIL_MODE == HIL_MODE_DISABLED
{"adc", test_adc},
{"gps", test_gps},
{"rawgps", test_rawgps},
{"imu", test_imu},
{"gyro", test_gyro},
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_SENSORS
{"adc", test_adc},
{"gps", test_gps},
{"imu", test_imu},
{"gyro", test_gyro},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_ATTITUDE
#endif
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
int8_t
test_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run();
}
void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
}
static int8_t
test_eedump(uint8_t argc, const Menu::arg *argv)
{
int i, j;
// hexdump the EEPROM
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
Serial.printf_P(PSTR("%04x:"), i);
for (j = 0; j < 16; j++)
Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
Serial.println();
}
return(0);
}
static int8_t
test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
#if THROTTLE_REVERSE == 1
Serial.println("Throttle is reversed in config: ");
delay(1000);
#endif
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
update_servo_switches();
servo_out[CH_ROLL] = reverse_roll * int(radio_in[CH_ROLL] - radio_trim(CH_ROLL)) * 9;
servo_out[CH_PITCH] = reverse_pitch * int(radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9;
servo_out[CH_RUDDER] = reverse_rudder * int(radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9;
// write out the servo PWM values
// ------------------------------
set_servos_4();
for(int y = 4; y < 8; y++) {
radio_out[y] = constrain(radio_in[y], radio_min(y), radio_max(y));
APM_RC.OutputCh(y, radio_out[y]); // send to Servos
}
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\t"), radio_in[CH_1], radio_in[CH_2], radio_in[CH_3], radio_in[CH_4], radio_in[CH_5], radio_in[CH_6], radio_in[CH_7], radio_in[CH_8]);
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (servo_out[CH_ROLL]/100), (servo_out[CH_PITCH]/100), servo_out[CH_THROTTLE], (servo_out[CH_RUDDER]/100));
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
#if BATTERY_EVENT == 1
for (int i = 0; i < 20; i++){
delay(20);
read_battery();
}
Serial.printf_P(PSTR("Volts: 1:"));
Serial.print(battery_voltage1, 4);
Serial.print(" 2:");
Serial.print(battery_voltage2, 4);
Serial.print(" 3:");
Serial.print(battery_voltage3, 4);
Serial.print(" 4:");
Serial.println(battery_voltage4, 4);
#else
Serial.printf_P(PSTR("Not enabled\n"));
#endif
return (0);
}
static int8_t
test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
Serial.println("Relay on");
relay_on();
delay(3000);
if(Serial.available() > 0){
return (0);
}
Serial.println("Relay off");
relay_off();
delay(3000);
if(Serial.available() > 0){
return (0);
}
}
}
void
test_wp_print(struct Location *cmd, byte index)
{
Serial.print("command #: ");
Serial.print(index, DEC);
Serial.print(" id: ");
Serial.print(cmd->id, DEC);
Serial.print(" p1: ");
Serial.print(cmd->p1, DEC);
Serial.print(" p2: ");
Serial.print(cmd->alt, DEC);
Serial.print(" p3: ");
Serial.print(cmd->lat, DEC);
Serial.print(" p4: ");
Serial.println(cmd->lng, DEC);
}
static int8_t
test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
ground_alt = 0;
//read_EEPROM_waypoint_info();
// save the alitude above home option
if(get(PARAM_CONFIG) & HOLD_ALT_ABOVE_HOME){
Serial.printf_P(PSTR("Hold altitude of %ldm\n"), get(PARAM_ALT_HOLD_HOME)/100);
}else{
Serial.printf_P(PSTR("Hold current altitude\n"));
}
Serial.printf_P(PSTR("%d waypoints\n"), get(PARAM_WP_TOTAL));
Serial.printf_P(PSTR("Hit radius: %d\n"), get(PARAM_WP_RADIUS));
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), get(PARAM_LOITER_RADIUS));
for(byte i = 0; i <= get(PARAM_WP_TOTAL); i++){
struct Location temp = get_wp_with_index(i);
test_wp_print(&temp, i);
}
return (0);
}
static int8_t
test_xbee(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
while(1){
delay(250);
// Timeout set high enough for X-CTU RSSI Calc over XBee @ 115200
Serial3.printf_P(PSTR("0123456789:;<=>?@ABCDEFGHIJKLMNO\n"));
//Serial.print("X");
// Default 32bit data from X-CTU Range Test
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.print("Control CH ");
Serial.println(FLIGHT_MODE_CHANNEL, DEC);
while(1){
delay(20);
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
Serial.printf_P(PSTR("Position %d\n"), switchPosition);
oldSwitchPosition = switchPosition;
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_dipswitches(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(100);
update_servo_switches();
if (mix_mode == 0) {
Serial.print("Mix:standard ");
Serial.print("\t reverse_roll: ");
Serial.print(reverse_roll, DEC);
Serial.print("\t reverse_pitch: ");
Serial.print(reverse_pitch, DEC);
Serial.print("\t reverse_rudder: ");
Serial.println(reverse_rudder, DEC);
} else {
Serial.print("Mix:elevons ");
Serial.print("\t reverse_elevons: ");
Serial.print(reverse_elevons, DEC);
Serial.print("\t reverse_ch1_elevon: ");
Serial.print(reverse_ch1_elevon, DEC);
Serial.print("\t reverse_ch2_elevon: ");
Serial.println(reverse_ch2_elevon, DEC);
}
if(Serial.available() > 0){
return (0);
}
}
}
//-------------------------------------------------------------------------------------------
// tests in this section are for real sensors or sensors that have been simulated
#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init();
delay(1000);
Serial.printf_P(PSTR("ADC\n"));
delay(1000);
while(1){
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%d\t"),adc.Ch(i));
Serial.println();
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(333);
// Blink GPS LED if we don't have a fix
// ------------------------------------
update_GPS_light();
gps.update();
if (gps.new_data){
Serial.print("Lat:");
Serial.print((float)gps.latitude/10000000, 10);
Serial.print(" Lon:");
Serial.print((float)gps.longitude/10000000, 10);
Serial.printf_P(PSTR("alt %ldm, #sats: %d\n"), gps.altitude/100, gps.num_sats);
}else{
Serial.print(".");
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_imu(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("Calibrating."));
imu.init_gyro();
print_hit_enter();
delay(1000);
while(1){
delay(20);
if (millis() - fast_loopTimer > 19) {
deltaMiliSeconds = millis() - fast_loopTimer;
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
dcm.update_DCM(G_Dt);
#if MAGNETOMETER == 1
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
medium_loopCounter = 0;
}
#endif
// We are using the IMU
// ---------------------
Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_gyro(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init();
delay(1000);
Serial.printf_P(PSTR("Gyro | Accel\n"));
delay(1000);
while(1){
Vector3f gyros = imu.get_gyro();
Vector3f accels = imu.get_accel();
Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), (int)gyros.x, (int)gyros.y, (int)gyros.z, (int)accels.x, (int)accels.y, (int)accels.z);
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_mag(uint8_t argc, const Menu::arg *argv)
{
#if MAGNETOMETER == DISABLED
Serial.printf_P(PSTR("Compass disabled\n"));
return (0);
#else
print_hit_enter();
while(1){
float heading;
delay(250);
compass.read();
heading = compass.calculate_heading(0,0);
Serial.printf_P(PSTR("Heading: ("));
Serial.print(ToDeg(heading));
Serial.printf_P(PSTR(") XYZ: ("));
Serial.print(compass.mag_x);
Serial.print(comma);
Serial.print(compass.mag_y);
Serial.print(comma);
Serial.print(compass.mag_z);
Serial.println(")");
}
#endif
}
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here
#if HIL_MODE == HIL_MODE_DISABLED
static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
#if AIRSPEED_SENSOR == DISABLED
Serial.printf_P(PSTR("airspeed disabled\n"));
return (0);
#else
print_hit_enter();
zero_airspeed();
while(1){
delay(20);
read_airspeed();
Serial.printf_P(PSTR("%dm/s\n"),airspeed/100);
if(Serial.available() > 0){
return (0);
}
}
#endif
}
static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv)
{
uint32_t sum = 0;
Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n"));
Serial.printf_P(PSTR("Note - the altitude displayed is relative to the start of this test\n"));
print_hit_enter();
Serial.printf_P(PSTR("\nCalibrating....\n"));
for (int i=1;i<301;i++) {
read_airpressure();
if(i>200)sum += abs_press_filt;
delay(10);
}
abs_press_gnd = (double)sum/100.0;
ground_alt = 0;
while(1){
delay(100);
read_airpressure();
//Serial.printf_P(PSTR("Alt: %dm, Raw: %d\n"), press_alt / 100, abs_press_filt); // Someone needs to fix the formatting here for long integers
Serial.print("Altitude: ");
Serial.print(press_alt/100.0,2);
Serial.print(" Raw pressure value: ");
Serial.println(abs_press_filt);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
if(Serial1.available() > 0) // Ok, let me see, the buffer is empty?
{
delay(60); // wait for it all
while(Serial1.available() > 0){
byte incoming = Serial1.read();
//Serial.print(incoming,DEC);
Serial.print(incoming, BYTE); // will output Hex values
//Serial.print(",");
}
Serial.println(" ");
}
if(Serial.available() > 0){
return (0);
}
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED

5
Tools/ArduTracker/updateParams
View File

@ -1,5 +0,0 @@
#!/bin/bash
echo Updating Parameters ...
awk -f paramgen.awk paramgen.in
echo Finished.
echo Did you increment firmware version?