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

APMrover v2.0 - tested on Traxxas Monster Jam Grinder XL-5 3602 

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
This commit is contained in:
Jean-Louis Naudin 2012-04-30 09:17:14 +02:00
parent ca8bc34c98
commit 77eea3a893
32 changed files with 12432 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
APMrover2/APM_Config.h Normal file
View File

@ -0,0 +1,47 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
// their default values, place the appropriate #define statements here.
// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented)
//#define SERIAL3_BAUD 38400
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
#define CLI_ENABLED ENABLED
#define CLI_SLIDER_ENABLED DISABLED
#define CLOSED_LOOP_NAV ENABLED
#define AUTO_WP_RADIUS ENABLED
//#include "APM_Config_HILmode.h" // for test in HIL mode with AeroSIM Rc 3.83
#include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder
// Radio setup:
// APM INPUT (Rec = receiver)
// Rec ch1: Roll
// Rec ch2: Throttle
// Rec ch3: Pitch
// Rec ch4: Yaw
// Rec ch5: not used
// Rec ch6:
// Rec ch7: Option channel to 2 positions switch
// Rec ch8: Mode channel to 3 positions switch
// APM OUTPUT
// Ch1: Wheel servo (direction)
// Ch2: not used
// Ch3: to the motor ESC
// Ch4: not used
// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable:
/*
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PORT 3
*/

41
APMrover2/APM_Config.h.org Normal file
View File

@ -0,0 +1,41 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
// their default values, place the appropriate #define statements here.
// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented)
//#define SERIAL3_BAUD 38400
// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
// # define APM2_BETA_HARDWARE
// You may also put an include statement here to point at another configuration file. This is convenient if you maintain
// different configuration files for different aircraft or HIL simulation. See the examples below
//#include "APM_Config_mavlink_hil.h"
//#include "Skywalker.h"
// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable:
/*
#define HIL_MODE HIL_MODE_ATTITUDE
*/
/*
// 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
// 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
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED
*/

954
APMrover2/APM_Config.h.reference Normal file
View File

@ -0,0 +1,954 @@
//
// Example and reference ArduPilot Mega configuration file
// =======================================================
//
// This file contains documentation and examples for 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_AUTO Auto detect GPS type (must be a supported GPS)
// GPS_PROTOCOL_NONE No GPS attached
// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU.
// GPS_PROTOCOL_MTK MediaTek-based GPS running the DIYDrones firmware 1.4
// GPS_PROTOCOL_MTK16 MediaTek-based GPS running the DIYDrones firmware 1.6
// 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?)
//
//#define GPS_PROTOCOL GPS_PROTOCOL_AUTO
//
//////////////////////////////////////////////////////////////////////////////
// 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
//
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER OPTIONAL
// MAG_ORIENTATION OPTIONAL
//
// Set MAGNETOMETER to ENABLED if you have a magnetometer attached.
// Set MAG_ORIENTATION to reflect the orientation you have the magnetometer mounted with respect to ArduPilotMega
//
// The default assumes that a magnetometer is not connected.
//
//#define MAGNETOMETER DISABLED
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
//
//////////////////////////////////////////////////////////////////////////////
// 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
//#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
//
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL OPTIONAL
// GCS_PORT OPTIONAL
// MAV_SYSTEM_ID 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_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 MAV_SYSTEM_ID is a unique identifier for this UAV. The default value is 1.
// If you will be flying multiple UAV's each should be assigned a different ID so
// that ground stations can tell them apart.
//
//#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
//#define GCS_PORT 3
//#define MAV_SYSTEM_ID 1
//
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
// SERIAL0_BAUD OPTIONAL
//
// Baudrate for the console port. Default is 115200bps.
//
// SERIAL3_BAUD OPTIONAL
//
// Baudrate for the telemetry port. Default is 57600bps.
//
//#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 low voltage or high discharge warnings.
// The default is DISABLED.
//
// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set.
//
// 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.
//
// CURR_AMPS_PER_VOLT OPTIONAL
// CURR_AMPS_OFFSET OPTIONAL
//
// The sensitivity of the current sensor. This must be scaled if a resistor is installed on APM
// for a voltage divider on input 2 (not recommended). The offset is used for current sensors with an offset
//
//
// HIGH_DISCHARGE OPTIONAL if BATTERY_EVENT is set.
//
// Value in milliamp-hours at which a warning should be triggered. Recommended value = 80% of
// battery capacity.
//
//#define BATTERY_EVENT DISABLED
//#define LOW_VOLTAGE 9.6
//#define VOLT_DIV_RATIO 3.56
//#define CURR_AMPS_PER_VOLT 27.32
//#define CURR_AMPS_OFFSET 0.0
//#define HIGH_DISCHARGE 1760
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE OPTIONAL
//
// In order to have accurate pressure and battery voltage readings, this
// value should be set to the voltage measured at the processor.
//
// See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail.
//
//#define INPUT_VOLTAGE 4.68 // 4.68 is the average value for a sample set. This is the value at the processor with 5.02 applied at the servo rail
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// 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.
// |
//air
//
// 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 STABILIZE
//#define FLIGHT_MODE_4 STABILIZE
//#define FLIGHT_MODE_5 MANUAL
//#define FLIGHT_MODE_6 MANUAL
//
//////////////////////////////////////////////////////////////////////////////
// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed
// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed
// then the flap position shall be flap_1_percent. If the speed setpoint is below flap_2_speed
// then the flap position shall be flap_2_percent. Speed setpoint is the current value of
// airspeed_cruise (if airspeed sensor enabled) or throttle_cruise (if no airspeed sensor)
// FLAP_1_PERCENT OPTIONAL
// FLAP_1_SPEED OPTIONAL
// FLAP_2_PERCENT OPTIONAL
// FLAP_2_SPEED OPTIONAL
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE OPTIONAL
// THROTTLE_FS_VALUE OPTIONAL
//
// The throttle failsafe allows you to configure a software failsafe activated
// by a setting on the throttle input channel (channel 3). Enabling this failsafe
// also enables "short failsafe" conditions (see below) based on loss of
// rc override control from the GCS
//
// 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 enabled.
//
// 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.
//
//#define THROTTLE_FAILSAFE ENABLED
//#define THROTTLE_FS_VALUE 950
//
//////////////////////////////////////////////////////////////////////////////
// GCS_HEARTBEAT_FAILSAFE OPTIONAL
// SHORT_FAILSAFE_ACTION OPTIONAL
// LONG_FAILSAFE_ACTION OPTIONAL
// There are two basic conditions which can trigger a failsafe. One is a loss of control signal.
// Normally this means a loss of the radio control RC signal. However if rc override from the
// GCS is in use then this can mean a loss of communication with the GCS. Such a failsafe will be
// classified as either short (greater than 1.5 seconds but less than 20) or long (greater than 20).
// Also, if GCS_HEARTBEAT_FAILSAFE is enabled and a heartbeat signal from the GCS has not been received
// in the preceeding 20 seconds then this will also trigger a "long" failsafe.
//
// The SHORT_FAILSAFE_ACTION and LONG_FAILSAFE_ACTION settings determines what APM will do when
// a failsafe mode is entered while flying in AUTO or LOITER mode. This is important in order to avoid
// accidental failsafe behaviour when flying waypoints that take the aircraft
// out of radio range.
//
// If SHORT_FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes,
// the aircraft will head for home in RTL mode. If the failsafe condition is
// resolved, it will return to AUTO or LOITER mode.
// If LONG_FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes,
// the aircraft will head for home in RTL mode. If the failsafe condition is
// resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home
// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER
// mode the aircraft will continue in that mode ignoring the failsafe condition.
// Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always
// enter a circling mode for short failsafe conditions and will be switched to RTL for long
// failsafe conditions. RTL mode is unaffected by failsafe conditions.
//
// The default is to have GCS Heartbeat failsafes DISABLED
// The default behaviour is to ignore failsafes in AUTO and LOITER modes.
//
//#define GCS_HEARTBEAT_FAILSAFE DISABLED
//#define SHORT_FAILSAFE_ACTION 0
//#define LONG_FAILSAFE_ACTION 0
//////////////////////////////////////////////////////////////////////////////
// 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 disable AUTO_TRIM.
//
//#define AUTO_TRIM DISABLED
//
//////////////////////////////////////////////////////////////////////////////
// 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.
//
// 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 pressure altitude.
//
//#define ALTITUDE_MIX 1
//
//////////////////////////////////////////////////////////////////////////////
// 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 12
//
//////////////////////////////////////////////////////////////////////////////
// MIN_GNDSPEED OPTIONAL
//
// The minimum ground speed in metres per second to maintain during
// cruise. A value of 0 will disable any attempt to maintain a minumum
// speed over ground.
//
#define MIN_GNDSPEED 0
//
//////////////////////////////////////////////////////////////////////////////
// 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.
//
//#define AIRSPEED_FBW_MIN 6
//#define AIRSPEED_FBW_MAX 22
//
//////////////////////////////////////////////////////////////////////////////
// Servo mapping
//
// 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 // percent
//#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.
//
// PITCH_TARGET
//
// The target pitch for cruise flight. When the APM measures this pitch
// value, the pitch error will be calculated to be 0 for the pitch PID
// control loop.
//
//#define HEAD_MAX 45
//#define PITCH_MAX 15
//#define PITCH_MIN -25
//#define PITCH_TARGET 0
//////////////////////////////////////////////////////////////////////////////
// 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).
// 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.
// Note units of this control loop are unusual. PID input is in m/s**2.
//
// 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 0.
//
// 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 meters.
//
//#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.
//
// Note units of this control loop are unusual. PID input is in m**2/s**2.
//
// 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. Range 1 to 100.
// Default is zero (disabled).
//
// P_TO_T OPTIONAL
//
// Pitch to throttle feed-forward gain. Default is 0.
//
// T_TO_P OPTIONAL
//
// Throttle to pitch feed-forward gain. Default is 0.
//
//#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 0
//#define T_TO_P 0
//
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
// XTRACK_GAIN OPTIONAL
//
// Crosstrack compensation in degrees per metre off track.
// Default value is 1.0 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 1 // deg/m
//#define XTRACK_ENTRY_ANGLE 30 // deg
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUGGING
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// 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
//#define LOG_CUR DISABLED
//
//////////////////////////////////////////////////////////////////////////////
// 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.
//
// USE_CURRENT_ALT OPTIONAL
// ALT_HOLD_HOME OPTIONAL
//
// When the user performs a factory reset on the APM, set the flag for weather
// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch.
// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow
// users to start using the APM without running the WaypointWriter first.
//
//#define WP_RADIUS_DEFAULT 30
//#define LOITER_RADIUS_DEFAULT 60
//#define USE_CURRENT_ALT FALSE
//#define ALT_HOLD_HOME 100
//
//////////////////////////////////////////////////////////////////////////////
// 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.

439
APMrover2/APM_Config_HILmode.h Normal file
View File

@ -0,0 +1,439 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE ORIGINAL X-PLANE INTERFACE
// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE!
#define FLIGHT_MODE_CHANNEL 8
#define X_PLANE ENABLED
//#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
//#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
#define GCS_PORT 3
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
//#define HIL_MODE HIL_MODE_DISABLED
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define FLIGHT_MODE_1 AUTO // pos 0 ---
#define FLIGHT_MODE_2 AUTO // pos 1
#define FLIGHT_MODE_3 STABILIZE // pos 2
#define FLIGHT_MODE_4 STABILIZE // pos 3 ---
#define FLIGHT_MODE_5 MANUAL // pos 4
#define FLIGHT_MODE_6 MANUAL // pos 5 ---
#define AUTO_TRIM ENABLED
#define THROTTLE_FAILSAFE DISABLED
#define AIRSPEED_SENSOR ENABLED
#define MAGNETOMETER DISABLED
#define LOGGING_ENABLED DISABLED
#define CH7_OPTION CH7_CH7_SAVE_WP
#define TURN_GAIN 5
#define CH7_OPTION CH7_SAVE_WP
#define FLIGHT_MODE_1 AUTO // pos 0 ---
#define FLIGHT_MODE_2 AUTO // pos 1
#define FLIGHT_MODE_3 STABILIZE // pos 2
#define FLIGHT_MODE_4 STABILIZE // pos 3 ---
#define FLIGHT_MODE_5 MANUAL // pos 4
#define FLIGHT_MODE_6 MANUAL // pos 5 ---
#define ENABLE_AIR_START DISABLED
#define MANUAL_LEVEL DISABLED
#define CLOSED_LOOP_NAV ENABLED // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
#define MAX_DIST 50 //300 // max distance (in m) for the HEADALT mode
#define SARSEC_BRANCH 50 // Long branch of the SARSEC pattern
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// 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 3
#define GSBOOST 0 // 60 // boost the throttle if ground speed is too low in case of windy conditions // 100
#define NUDGE_OFFSET 0 //1603 // nudge_offset to get a good sustained speed in autonomous flight
#define MIN_GNDSPEED 3
//////////////////////////////////////////////////////////////////////////////
// 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.
//
#define AIRSPEED_FBW_MIN 6
#define AIRSPEED_FBW_MAX 35
//
//////////////////////////////////////////////////////////////////////////////
// Servo mapping
//
// 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 // percent
#define THROTTLE_CRUISE 1 // 40
#define THROTTLE_MAX 100
//////////////////////////////////////////////////////////////////////////////
// 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
#define THROTTLE_FAILSAFE DISABLED
//#define ENABLE_AIR_START 0
//////////////////////////////////////////////////////////////////////////////
// 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.
//
// PITCH_TARGET
//
// The target pitch for cruise flight. When the APM measures this pitch
// value, the pitch error will be calculated to be 0 for the pitch PID
// control loop.
//
#define HEAD_MAX 80
#define PITCH_MAX 15
#define PITCH_MIN -20 //-25
#define PITCH_TARGET 0
//////////////////////////////////////////////////////////////////////////////
// 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.
// Note units of this control loop are unusual. PID input is in m/s**2.
//
// 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 0.
//
// 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.0
#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.0
#define SERVO_PITCH_I 0.0
#define SERVO_PITCH_D 0.0
#define SERVO_PITCH_INT_MAX 5
#define PITCH_COMP 0.0
#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.0
//
//////////////////////////////////////////////////////////////////////////////
// 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 meters.
//
#define NAV_ROLL_P 0.7
#define NAV_ROLL_I 0.001
#define NAV_ROLL_D 0.06
#define NAV_ROLL_INT_MAX 5
#define NAV_PITCH_ASP_P 0.0
#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.0
#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.
//
// Note units of this control loop are unusual. PID input is in m**2/s**2.
//
// 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. Default is 0.
//
// T_TO_P OPTIONAL
//
// Throttle to pitch feed-forward gain. Default is 0.
//
#define THROTTLE_TE_P 0.1
#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 0.0
#define T_TO_P 0
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
// XTRACK_GAIN OPTIONAL
//
// Crosstrack compensation in degrees per metre off track.
// Default value is 1.0 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 1 // deg/m
#define XTRACK_ENTRY_ANGLE 20 // deg
/////////////////////////////////////////////////////////////////////////////
// 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.
//
// USE_CURRENT_ALT OPTIONAL
// ALT_HOLD_HOME OPTIONAL
//
// When the user performs a factory reset on the APM, set the flag for weather
// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch.
// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow
// users to start using the APM without running the WaypointWriter first.
//
#define WP_RADIUS_DEFAULT 3 // meters
#define LOITER_RADIUS_DEFAULT 10 // 60
#define USE_CURRENT_ALT TRUE
#define ALT_HOLD_HOME 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.2
//

430
APMrover2/APM_Config_Rover.h Normal file
View File

@ -0,0 +1,430 @@
// CONFIG FILE FOR APM_Rover project by Jean-Louis Naudin
//
#define GCS_PORT 3
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK // QGroundControl protocol
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE // No GCS protocol to save memory
#define HIL_MODE HIL_MODE_DISABLED
#define MAV_SYSTEM_ID 1
// Add a ground start delay in seconds
//#define GROUND_START_DELAY 1
#define AIRSPEED_SENSOR DISABLED
#define MAGNETOMETER ENABLED
#define LOGGING_ENABLED DISABLED
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
#define SERIAL0_BAUD 115200
#define SERIAL3_BAUD 115200
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
#define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#define CH7_OPTION CH7_SAVE_WP
#define TUNING_OPTION TUN_NONE
#define FLIGHT_MODE_1 AUTO // pos 0 ---
#define FLIGHT_MODE_2 AUTO // pos 1
#define FLIGHT_MODE_3 STABILIZE // pos 2
#define FLIGHT_MODE_4 STABILIZE // pos 3 ---
#define FLIGHT_MODE_5 MANUAL // pos 4
#define FLIGHT_MODE_6 MANUAL // pos 5 ---
#define ENABLE_AIR_START DISABLED
#define MANUAL_LEVEL DISABLED
#define TURN_GAIN 5
#define CLOSED_LOOP_NAV ENABLED // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
#define AUTO_WP_RADIUS ENABLED
#define MAX_DIST 50 //300 // max distance (in m) for the HEADALT mode
#define SARSEC_BRANCH 50 // Long branch of the SARSEC pattern
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// 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 3
#define GSBOOST 0
#define NUDGE_OFFSET 0
#define MIN_GNDSPEED 3
//////////////////////////////////////////////////////////////////////////////
// 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.
//
#define AIRSPEED_FBW_MIN 6
#define AIRSPEED_FBW_MAX 35
//
//////////////////////////////////////////////////////////////////////////////
// Servo mapping
//
// 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 // percent
#define THROTTLE_CRUISE 3 // 40
#define THROTTLE_MAX 100
//////////////////////////////////////////////////////////////////////////////
// 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
#define THROTTLE_FAILSAFE DISABLED
//#define ENABLE_AIR_START 0
//////////////////////////////////////////////////////////////////////////////
// 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.
//
// PITCH_TARGET
//
// The target pitch for cruise flight. When the APM measures this pitch
// value, the pitch error will be calculated to be 0 for the pitch PID
// control loop.
//
#define HEAD_MAX 80
#define PITCH_MAX 15
#define PITCH_MIN -20 //-25
#define PITCH_TARGET 0
//////////////////////////////////////////////////////////////////////////////
// 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.
// Note units of this control loop are unusual. PID input is in m/s**2.
//
// 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 0.
//
// 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.0
#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.0
#define SERVO_PITCH_I 0.0
#define SERVO_PITCH_D 0.0
#define SERVO_PITCH_INT_MAX 5
#define PITCH_COMP 0.0
#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.0
//
//////////////////////////////////////////////////////////////////////////////
// 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 meters.
//
#define NAV_ROLL_P 0.7
#define NAV_ROLL_I 0.001
#define NAV_ROLL_D 0.06
#define NAV_ROLL_INT_MAX 5
#define NAV_PITCH_ASP_P 0.0
#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.0
#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.
//
// Note units of this control loop are unusual. PID input is in m**2/s**2.
//
// 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. Default is 0.
//
// T_TO_P OPTIONAL
//
// Throttle to pitch feed-forward gain. Default is 0.
//
#define THROTTLE_TE_P 0.1
#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 0.0
#define T_TO_P 0
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
// XTRACK_GAIN OPTIONAL
//
// Crosstrack compensation in degrees per metre off track.
// Default value is 1.0 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 1 // deg/m
#define XTRACK_ENTRY_ANGLE 20 // deg
/////////////////////////////////////////////////////////////////////////////
// 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.
//
// USE_CURRENT_ALT OPTIONAL
// ALT_HOLD_HOME OPTIONAL
//
// When the user performs a factory reset on the APM, set the flag for weather
// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch.
// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow
// users to start using the APM without running the WaypointWriter first.
//
#define WP_RADIUS_DEFAULT 3 // meters
#define LOITER_RADIUS_DEFAULT 10 // 60
#define USE_CURRENT_ALT TRUE
#define ALT_HOLD_HOME 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.2
//

31
APMrover2/APM_Config_mavlink_hil.h Normal file
View File

@ -0,0 +1,31 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE MAVLINK HIL INTERFACE
// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE!
// 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
// 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
// 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
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED

1135
APMrover2/APMrover2.pde Normal file
View File

File diff suppressed because it is too large Load Diff

208
APMrover2/Attitude.pde Normal file
View File

@ -0,0 +1,208 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//****************************************************************
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************
static void stabilize()
{
float ch1_inf = 1.0;
// Calculate desired servo output for the turn // Wheels Direction
// ---------------------------------------------
g.channel_roll.servo_out = nav_roll;
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
if ((control_mode < FLY_BY_WIRE_A) ||
(ENABLE_STICK_MIXING == 1 &&
geofence_stickmixing() &&
control_mode > FLY_BY_WIRE_B &&
failsafe == FAILSAFE_NONE)) {
// TODO: use RC_Channel control_mix function?
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
ch1_inf = fabs(ch1_inf);
ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
// scale the sensor input based on the stick input
// -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf;
// Mix in stick inputs
// -------------------
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
//Serial.printf_P(PSTR(" servo_out[CH_ROLL] "));
//Serial.println(servo_out[CH_ROLL],DEC);
}
g.channel_roll.servo_out = g.channel_roll.servo_out * g.turn_gain;
g.channel_rudder.servo_out = g.channel_roll.servo_out;
}
static void crash_checker()
{
if(ahrs.pitch_sensor < -4500){
crash_timer = 255;
}
if(crash_timer > 0)
crash_timer--;
}
static void calc_throttle()
{
int throttle_target = g.throttle_cruise + throttle_nudge + 50;
// Normal airspeed target
target_airspeed = g.airspeed_cruise;
groundspeed_error = target_airspeed - (float)ground_speed;
g.channel_throttle.servo_out = throttle_target + g.pidTeThrottle.get_pid(groundspeed_error, dTnav);
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
}
/*****************************************
* Calculate desired turn angles (in medium freq loop)
*****************************************/
static void calc_nav_roll()
{
// Adjust gain based on ground speed
nav_gain_scaler = (float)ground_speed / (g.airspeed_cruise * 100.0);
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
// Calculate the required turn of the wheels rover
// ----------------------------------------
// negative error = left turn
// positive error = right turn
nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
}
/*****************************************
* Roll servo slew limit
*****************************************/
/*
float roll_slew_limit(float servo)
{
static float last;
float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f);
last = servo;
return temp;
}*/
/*****************************************
* Throttle slew limit
*****************************************/
static void throttle_slew_limit()
{
static int last = 1000;
if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit
float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000
g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
last = g.channel_throttle.radio_out;
}
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_I(void)
{
g.pidNavRoll.reset_I();
g.pidTeThrottle.reset_I();
// g.pidAltitudeThrottle.reset_I();
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
static void set_servos(void)
{
int flapSpeedSource = 0;
// vectorize the rc channels
RC_Channel_aux* rc_array[NUM_CHANNELS];
rc_array[CH_1] = NULL;
rc_array[CH_2] = NULL;
rc_array[CH_3] = NULL;
rc_array[CH_4] = NULL;
rc_array[CH_5] = &g.rc_5;
rc_array[CH_6] = &g.rc_6;
rc_array[CH_7] = &g.rc_7;
rc_array[CH_8] = &g.rc_8;
if((control_mode == MANUAL) || (control_mode == STABILIZE)){
// do a direct pass through of radio values
g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_roll.radio_in;
} else {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
// convert 0 to 100% into PWM
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
// g.channel_throttle.calc_pwm();
/* TO DO - fix this for RC_Channel library
#if THROTTLE_REVERSE == 1
radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
#endif
*/
}
if (control_mode >= FLY_BY_WIRE_B) {
g.channel_throttle.calc_pwm();
/* only do throttle slew limiting in modes where throttle
control is automatic */
throttle_slew_limit();
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
// Route configurable aux. functions to their respective servos
g.rc_5.output_ch(CH_5);
g.rc_6.output_ch(CH_6);
g.rc_7.output_ch(CH_7);
g.rc_8.output_ch(CH_8);
#endif
}
static void demo_servos(byte i) {
while(i > 0){
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
APM_RC.OutputCh(1, 1400);
mavlink_delay(400);
APM_RC.OutputCh(1, 1600);
mavlink_delay(200);
APM_RC.OutputCh(1, 1500);
#endif
mavlink_delay(400);
i--;
}
}

178
APMrover2/GCS.h Normal file
View File

@ -0,0 +1,178 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file GCS.h
/// @brief Interface definition for the various Ground Control System protocols.
#ifndef __GCS_H
#define __GCS_H
#include <FastSerial.h>
#include <AP_Common.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(FastSerial *port) {
_port = port;
initialised = true;
}
/// Update GCS state.
///
/// This may involve checking for received bytes on the stream,
/// or sending additional periodic messages.
void update(void) {}
/// Send a message with a single numeric parameter.
///
/// This may be a standalone message, or the GCS driver may
/// have its own way of locating additional parameters to send.
///
/// @param id ID of the message to send.
/// @param param Explicit message parameter.
///
void send_message(enum ap_message id) {}
/// Send a text message.
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(gcs_severity severity, const char *str) {}
/// Send a text message with a PSTR()
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(gcs_severity severity, const prog_char_t *str) {}
// 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);
// set to true if this GCS link is active
bool initialised;
protected:
/// The stream we are communicating over
FastSerial *_port;
};
//
// GCS class definitions.
//
// These are here so that we can declare the GCS object early in the sketch
// and then reference it statically rather than via a pointer.
//
///
/// @class GCS_MAVLINK
/// @brief The mavlink protocol for qgroundcontrol
///
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK();
void update(void);
void init(FastSerial *port);
void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str);
void send_text(gcs_severity severity, const prog_char_t *str);
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
AP_Param *_queued_parameter; ///< next parameter to be sent in queue
enum ap_var_type _queued_parameter_type; ///< type of the next parameter
AP_Param::ParamToken _queued_parameter_token;///AP_Param token for next() call
uint16_t _queued_parameter_index; ///< next queued parameter's index
uint16_t _queued_parameter_count; ///< saved count of parameters for queued send
/// Count the number of reportable parameters.
///
/// Not all parameters can be reported via MAVlink. We count the number that are
/// so that we can report to a GCS the number of parameters it should expect when it
/// requests the full set.
///
/// @return The number of reportable parameters.
///
uint16_t _count_parameters(); ///< count reportable parameters
uint16_t _parameter_count; ///< cache of reportable parameters
mavlink_channel_t chan;
uint16_t packet_drops;
#if CLI_ENABLED == ENABLED
// this allows us to detect the user wanting the CLI to start
uint8_t crlf_count;
#endif
// 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
uint32_t waypoint_timelast_request; // milliseconds
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
// data stream rates
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;
AP_Int16 streamRateRawController;
AP_Int16 streamRatePosition;
AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
};
#endif // __GCS_H

2181
APMrover2/GCS_Mavlink.pde Normal file
View File

File diff suppressed because it is too large Load Diff

696
APMrover2/Log.pde Normal file
View File

@ -0,0 +1,696 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if LOGGING_ENABLED == ENABLED
// 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 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);
static int16_t cur_throttle =0;
// 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>"
" erase (all logs)\n"
" enable <name> | all\n"
" disable <name> | all\n"
"\n"));
return 0;
}*/
// 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
static const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
{"disable", select_logs}
};
// 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;
int temp;
int last_log_num = DataFlash.find_last_log();
uint16_t num_logs = DataFlash.get_num_logs();
Serial.printf_P(PSTR("logs enabled: "));
if (0 == g.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 (g.log_bitmask & MASK_LOG_ ## _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);
PLOG(CUR);
#undef PLOG
}
Serial.println();
if (num_logs == 0) {
Serial.printf_P(PSTR("\nNo logs\n\n"));
}else{
Serial.printf_P(PSTR("\n%d logs\n"), num_logs);
for(int i=num_logs;i>=1;i--) {
int last_log_start = log_start, last_log_end = log_end;
temp = last_log_num-i+1;
DataFlash.get_log_boundaries(temp, log_start, log_end);
Serial.printf_P(PSTR("Log %d, start %d, end %d\n"), temp, log_start, log_end);
if (last_log_start == log_start && last_log_end == log_end) {
// we are printing bogus logs
break;
}
}
Serial.println();
}
return(true);
}
static int8_t
dump_log(uint8_t argc, const Menu::arg *argv)
{
int 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 = DataFlash.find_last_log();
if (dump_log == -2) {
for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) {
DataFlash.StartRead(count);
Serial.printf_P(PSTR("DF page, log file #, log page: %d,\t"), count);
Serial.printf_P(PSTR("%d,\t"), DataFlash.GetFileNumber());
Serial.printf_P(PSTR("%d\n"), DataFlash.GetFilePage());
}
return(-1);
} else if (dump_log <= 0) {
Serial.printf_P(PSTR("dumping all\n"));
Log_Read(1, DataFlash.df_NumPages);
return(-1);
} else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log number\n"));
return(-1);
}
DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"),
dump_log,
dump_log_start,
dump_log_end);
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Done\n"));
return 0;
}
void erase_callback(unsigned long t) {
mavlink_delay(t);
if (DataFlash.GetWritePage() % 128 == 0) {
Serial.printf_P(PSTR("+"));
}
}
static void do_erase_logs(void)
{
Serial.printf_P(PSTR("\nErasing log...\n"));
DataFlash.EraseAll(erase_callback);
Serial.printf_P(PSTR("\nLog erased.\n"));
}
static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv)
{
in_mavlink_delay = true;
do_erase_logs();
in_mavlink_delay = false;
return 0;
}
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 = ~0;
} else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST);
TARG(ATTITUDE_MED);
TARG(GPS);
TARG(PM);
TARG(CTUN);
TARG(NTUN);
TARG(MODE);
TARG(RAW);
TARG(CMD);
TARG(CUR);
#undef TARG
}
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
g.log_bitmask.set_and_save(g.log_bitmask | bits);
}else{
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
}
return(0);
}
static int8_t
process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int16_t log_roll, int16_t 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
#if HIL_MODE != HIL_MODE_ATTITUDE
static 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((int16_t)mainLoop_count);
DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(0);
DataFlash.WriteByte(imu.adc_constraints);
DataFlash.WriteByte(ahrs.renorm_range_count);
DataFlash.WriteByte(ahrs.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt(1);
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000));
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000));
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000));
DataFlash.WriteInt(pmTest1);
DataFlash.WriteByte(END_BYTE);
}
#endif
// Write a command processing packet. Total length : 19 bytes
//void Log_Write_Cmd(byte num, byte id, byte p1, int32_t alt, int32_t lat, int32_t lng)
static 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);
}
static void Log_Write_Startup(byte type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type);
DataFlash.WriteByte(g.command_total);
DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_cmd_with_index(0);
Log_Write_Cmd(0, &cmd);
for (int i = 1; i <= g.command_total; i++){
cmd = get_cmd_with_index(i);
Log_Write_Cmd(i, &cmd);
}
}
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static 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)(g.channel_roll.servo_out));
DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)ahrs.roll_sensor);
DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)ahrs.pitch_sensor);
DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteByte(END_BYTE);
}
#endif
// Write a navigation tuning packet. Total length : 18 bytes
static void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((uint16_t)ahrs.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
static 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
static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
int32_t log_Ground_Speed, int32_t 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.WriteInt(sonar_alt); // This one is just temporary for testing out sonar in fixed wing
DataFlash.WriteLong(log_mix_alt);
DataFlash.WriteLong(log_gps_alt);
DataFlash.WriteLong(log_Ground_Speed);
DataFlash.WriteLong(log_Ground_Course);
DataFlash.WriteInt((int16_t)Vz);
DataFlash.WriteInt((int16_t)delta_Vz);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteByte(END_BYTE);
}
// Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static 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
static void Log_Write_Current()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.channel_throttle.control_in);
DataFlash.WriteInt((int)(battery_voltage1 * 100.0));
DataFlash.WriteInt((int)(current_amps1 * 100.0));
DataFlash.WriteInt((int)current_total1);
DataFlash.WriteByte(END_BYTE);
}
// Read a Current packet
static void Log_Read_Current()
{
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
DataFlash.ReadInt());
}
// Read an control tuning packet
static void Log_Read_Control_Tuning()
{
float logvar;
Serial.printf_P(PSTR("CTUN: "));
for (int y = 1; y < 10; y++) {
logvar = DataFlash.ReadInt();
if(y == 7) cur_throttle = logvar;
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
Serial.print(logvar);
Serial.print(comma);
Serial.print(" ");
}
Serial.println("");
}
// Read a nav tuning packet
static void Log_Read_Nav_Tuning()
{
int16_t d[7];
for (int8_t i=0; i<7; i++) {
d[i] = DataFlash.ReadInt();
}
Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
d[0]/100.0,
d[1],
((uint16_t)d[2])/100.0,
((uint16_t)d[3])/100.0,
d[4]/100.0,
d[5]/100.0,
d[5]/1000.0);
}
// Read a performance packet
static void Log_Read_Performance()
{
int32_t pm_time;
int logvar;
Serial.printf_P(PSTR("PM: "));
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
Serial.print(comma);
for (int y = 1; y <= 12; y++) {
if(y < 3 || y > 7){
logvar = DataFlash.ReadInt();
}else{
logvar = DataFlash.ReadByte();
}
Serial.print(logvar);
Serial.print(comma);
Serial.print(" ");
}
Serial.println("");
}
// Read a command processing packet
static void Log_Read_Cmd()
{
byte logvarb;
int32_t logvarl;
Serial.printf_P(PSTR("CMD: "));
for(int i = 1; i < 4; i++) {
logvarb = DataFlash.ReadByte();
Serial.print(logvarb, DEC);
Serial.print(comma);
Serial.print(" ");
}
for(int i = 1; i < 4; i++) {
logvarl = DataFlash.ReadLong();
Serial.print(logvarl, DEC);
Serial.print(comma);
Serial.print(" ");
}
Serial.println("");
}
static void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
if (logbyte == TYPE_AIRSTART_MSG)
Serial.printf_P(PSTR("AIR START - "));
else if (logbyte == TYPE_GROUNDSTART_MSG)
Serial.printf_P(PSTR("GROUND START - "));
else
Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
}
// Read an attitude packet
static void Log_Read_Attitude()
{
int16_t d[3];
d[0] = DataFlash.ReadInt();
d[1] = DataFlash.ReadInt();
d[2] = DataFlash.ReadInt();
Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
d[0], d[1],
(uint16_t)d[2]);
}
// Read a mode packet
static void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD: "));
Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
}
// Read a GPS packet
static void Log_Read_GPS()
{
int32_t l[7];
byte b[2];
int16_t i,j,k,m;
l[0] = DataFlash.ReadLong();
b[0] = DataFlash.ReadByte();
b[1] = DataFlash.ReadByte();
l[1] = DataFlash.ReadLong();
l[2] = DataFlash.ReadLong();
i = DataFlash.ReadInt();
l[3] = DataFlash.ReadLong();
l[4] = DataFlash.ReadLong();
l[5] = DataFlash.ReadLong();
l[6] = DataFlash.ReadLong();
j = DataFlash.ReadInt();
k = DataFlash.ReadInt();
m = DataFlash.ReadInt();
/*
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
(long)l[0], (int)b[0], (int)b[1],
l[1]/t7, l[2]/t7,
(int)i,
l[3]/100.0, l[4]/100.0, l[5]/100.0, l[6]/100.0); */
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
(long)l[0], (int)b[0], (int)b[1],
l[1]/t7, l[2]/t7,
l[4]/100.0, l[3]/100.0, l[5]/100.0, l[6]/100.0);
Serial.printf_P(PSTR("THP: %4.7f, %4.7f, %4.4f, %2.1f, %2.1f, %2.1f, %d\n"),
l[1]/t7, l[2]/t7, l[3]/100.0,
(float)j/100.0, (float)k/100.0, (float)m/100.0, cur_throttle);
}
// Read a raw accel/gyro packet
static void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW: "));
for (int y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar);
Serial.print(comma);
Serial.print(" ");
}
Serial.println("");
}
// Read the DataFlash log memory : Packet Parser
static void Log_Read(int16_t start_page, int16_t end_page)
{
int packet_count = 0;
#ifdef AIRFRAME_NAME
Serial.printf_P(PSTR((AIRFRAME_NAME)
#endif
Serial.printf_P(PSTR("\n" THISFIRMWARE
"\nFree RAM: %u\n"),
memcheck_available_memory());
if(start_page > end_page)
{
packet_count = Log_Read_Process(start_page, DataFlash.df_NumPages);
packet_count += Log_Read_Process(1, end_page);
} else {
packet_count = Log_Read_Process(start_page, end_page);
}
Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count);
}
// Read the DataFlash log memory : Packet Parser
static int Log_Read_Process(int16_t start_page, int16_t end_page)
{
byte data;
byte log_step = 0;
int page = start_page;
int packet_count = 0;
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_CURRENT_MSG){
Log_Read_Current();
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.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
log_step = 0; // Restart, we have a problem...
}
}
break;
case 3:
if(data == END_BYTE){
packet_count++;
}else{
Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
}
log_step = 0; // Restart sequence: new packet...
break;
}
page = DataFlash.GetPage();
}
return packet_count;
}
#else // LOGGING_ENABLED
// dummy functions
static void Log_Write_Mode(byte mode) {}
static void Log_Write_Startup(byte type) {}
static void Log_Write_Cmd(byte num, struct Location *wp) {}
static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {}
static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) {}
static void Log_Write_Performance() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {}
static void Log_Write_Control_Tuning() {}
static void Log_Write_Raw() {}
#endif // LOGGING_ENABLED

35
APMrover2/Makefile Normal file
View File

@ -0,0 +1,35 @@
include ../libraries/AP_Common/Arduino.mk
nologging:
make -f Makefile EXTRAFLAGS="-DLOGGING_ENABLED=DISABLED"
nogps:
make -f Makefile EXTRAFLAGS="-DGPS_PROTOCOL=GPS_PROTOCOL_NONE -DLOGGING_ENABLED=DISABLED"
hil:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_SLIDER_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
hilsensors:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCLI_SLIDER_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
hilnocli:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
heli:
make -f Makefile EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
apm2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
apm2beta:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
mavlink10:
make -f Makefile EXTRAFLAGS="-DMAVLINK10"
sitl:
make -f ../libraries/Desktop/Makefile.desktop
etags:
cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)

510
APMrover2/Parameters.h Normal file
View File

@ -0,0 +1,510 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 13;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches.
//
static const uint16_t k_software_type = 0; // 0 for APM trunk
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
// Misc
//
k_param_auto_trim,
k_param_switch_enable,
k_param_log_bitmask,
k_param_pitch_trim,
k_param_mix_mode,
k_param_reverse_elevons,
k_param_reverse_ch1_elevon,
k_param_reverse_ch2_elevon,
k_param_flap_1_percent,
k_param_flap_1_speed,
k_param_flap_2_percent,
k_param_flap_2_speed,
k_param_num_resets,
k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change
k_param_reset_switch_chan,
k_param_manual_level,
// 110: Telemetry control
//
k_param_gcs0 = 110, // stream rates for port0
k_param_gcs3, // stream rates for port3
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
// 120: Fly-by-wire control
//
k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max,
k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
//
// 130: Sensor parameters
//
k_param_imu = 130, // sensor calibration
k_param_altitude_mix,
k_param_airspeed_ratio,
k_param_ground_temperature,
k_param_ground_pressure,
k_param_compass_enabled,
k_param_compass,
k_param_battery_monitoring,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
k_param_input_voltage,
k_param_pack_capacity,
k_param_airspeed_offset,
// k_param_sonar_enabled,
k_param_airspeed_enabled,
//
// 150: Navigation parameters
//
k_param_crosstrack_gain = 150,
k_param_crosstrack_entry_angle,
k_param_roll_limit,
k_param_pitch_limit_max,
k_param_pitch_limit_min,
k_param_airspeed_cruise,
k_param_RTL_altitude,
k_param_inverted_flight_ch,
k_param_min_gndspeed,
k_param_ch7_option,
//
// 160: Radio settings
//
k_param_channel_roll = 160,
k_param_channel_pitch,
k_param_channel_throttle,
k_param_channel_rudder,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled, // 170
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_short_fs_action,
k_param_long_fs_action,
k_param_gcs_heartbeat_fs_enabled,
k_param_throttle_slewrate,
// ************************************************************
// 180: APMrover parameters - JLN update
k_param_closed_loop_nav,
k_param_auto_wp_radius,
k_param_nudgeoffset,
k_param_turn_gain,
// ************************************************************
//
// 200: Feed-forward gains
//
k_param_kff_pitch_compensation = 200,
k_param_kff_rudder_mix,
k_param_kff_pitch_to_throttle,
k_param_kff_throttle_to_pitch,
//
// 210: flight modes
//
k_param_flight_mode_channel = 210,
k_param_flight_mode1,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
//
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_command_total,
k_param_command_index,
k_param_waypoint_radius,
k_param_loiter_radius,
k_param_fence_action,
k_param_fence_total,
k_param_fence_channel,
k_param_fence_minalt,
k_param_fence_maxalt,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from command to roll command deviation from trim
// (bank to turn strategy)
//
k_param_pidNavRoll = 240,
// Roll-to-servo PID:
// roll error from command to roll servo deviation from trim
// (tracks commanded bank angle)
//
k_param_pidServoRoll,
//
// Pitch control
//
// Pitch-to-servo PID:
// pitch error from command to pitch servo deviation from trim
// (front-side strategy)
//
k_param_pidServoPitch,
// Airspeed-to-pitch PID:
// airspeed error from command to pitch servo deviation from trim
// (back-side strategy)
//
k_param_pidNavPitchAirspeed,
//
// Yaw control
//
// Yaw-to-servo PID:
// yaw rate error from command to yaw servo deviation from trim
// (stabilizes dutch roll)
//
k_param_pidServoRudder,
//
// Throttle control
//
// Energy-to-throttle PID:
// total energy error from command to throttle servo deviation from trim
// (throttle back-side strategy alternative)
//
k_param_pidTeThrottle,
// Altitude-to-pitch PID:
// altitude error from command to pitch servo deviation from trim
// (throttle front-side strategy alternative)
//
k_param_pidNavPitchAltitude,
// 254,255: reserved
};
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
// Feed-forward gains
//
AP_Float kff_pitch_compensation;
AP_Float kff_rudder_mix;
AP_Float kff_pitch_to_throttle;
AP_Float kff_throttle_to_pitch;
// Crosstrack navigation
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
// Estimation
//
AP_Float altitude_mix;
AP_Float airspeed_ratio;
AP_Int16 airspeed_offset;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 command_total;
AP_Int8 command_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
#if GEOFENCE_ENABLED == ENABLED
AP_Int8 fence_action;
AP_Int8 fence_total;
AP_Int8 fence_channel;
AP_Int16 fence_minalt; // meters
AP_Int16 fence_maxalt; // meters
#endif
// Fly-by-wire
//
AP_Int8 flybywire_airspeed_min;
AP_Int8 flybywire_airspeed_max;
AP_Int16 FBWB_min_altitude;
// Throttle
//
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_slewrate;
AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value;
AP_Int8 throttle_cruise;
// Failsafe
AP_Int8 short_fs_action;
AP_Int8 long_fs_action;
AP_Int8 gcs_heartbeat_fs_enabled;
// Flight modes
//
AP_Int8 flight_mode_channel;
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
// Navigational maneuvering limits
//
AP_Int16 roll_limit;
AP_Int16 pitch_limit_max;
AP_Int16 pitch_limit_min;
// Misc
//
AP_Int8 auto_trim;
AP_Int8 switch_enable;
AP_Int8 mix_mode;
AP_Int8 reverse_elevons;
AP_Int8 reverse_ch1_elevon;
AP_Int8 reverse_ch2_elevon;
AP_Int16 num_resets;
AP_Int16 log_bitmask;
AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change
AP_Int8 reset_switch_chan;
AP_Int8 manual_level;
AP_Int16 airspeed_cruise;
AP_Int16 min_gndspeed;
AP_Int8 ch7_option;
AP_Int16 pitch_trim;
AP_Int16 RTL_altitude;
AP_Int16 ground_temperature;
AP_Int32 ground_pressure;
AP_Int8 compass_enabled;
AP_Int16 angle_of_attack;
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
AP_Float volt_div_ratio;
AP_Float curr_amp_per_volt;
AP_Float input_voltage;
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
// AP_Int8 sonar_enabled;
AP_Int8 airspeed_enabled;
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;
AP_Int8 flap_2_percent;
AP_Int8 flap_2_speed;
// ************ ThermoPilot parameters ************************
// - JLN update
AP_Int8 closed_loop_nav;
AP_Int8 auto_wp_radius;
AP_Int16 nudgeoffset;
AP_Int16 turn_gain;
// ************************************************************
// RC channels
RC_Channel channel_roll;
RC_Channel channel_pitch;
RC_Channel channel_throttle;
RC_Channel channel_rudder;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
// PID controllers
//
PID pidNavRoll;
PID pidServoRoll;
PID pidServoPitch;
PID pidNavPitchAirspeed;
PID pidServoRudder;
PID pidTeThrottle;
PID pidNavPitchAltitude;
Parameters() :
format_version (k_format_version),
software_type (k_software_type),
sysid_this_mav (MAV_SYSTEM_ID),
sysid_my_gcs (255),
serial3_baud (SERIAL3_BAUD/1000),
kff_pitch_compensation (PITCH_COMP),
kff_rudder_mix (RUDDER_MIX),
kff_pitch_to_throttle (P_TO_T),
kff_throttle_to_pitch (T_TO_P),
crosstrack_gain (XTRACK_GAIN_SCALED),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE),
altitude_mix (ALTITUDE_MIX),
airspeed_ratio (AIRSPEED_RATIO),
airspeed_offset (0),
/* XXX waypoint_mode missing here */
command_total (0),
command_index (0),
waypoint_radius (WP_RADIUS_DEFAULT),
loiter_radius (LOITER_RADIUS_DEFAULT),
#if GEOFENCE_ENABLED == ENABLED
fence_action (0),
fence_total (0),
fence_channel (0),
fence_minalt (0),
fence_maxalt (0),
#endif
flybywire_airspeed_min (AIRSPEED_FBW_MIN),
flybywire_airspeed_max (AIRSPEED_FBW_MAX),
throttle_min (THROTTLE_MIN),
throttle_max (THROTTLE_MAX),
throttle_slewrate (THROTTLE_SLEW_LIMIT),
throttle_fs_enabled (THROTTLE_FAILSAFE),
throttle_fs_value (THROTTLE_FS_VALUE),
throttle_cruise (THROTTLE_CRUISE),
short_fs_action (SHORT_FAILSAFE_ACTION),
long_fs_action (LONG_FAILSAFE_ACTION),
gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE),
flight_mode_channel (FLIGHT_MODE_CHANNEL),
flight_mode1 (FLIGHT_MODE_1),
flight_mode2 (FLIGHT_MODE_2),
flight_mode3 (FLIGHT_MODE_3),
flight_mode4 (FLIGHT_MODE_4),
flight_mode5 (FLIGHT_MODE_5),
flight_mode6 (FLIGHT_MODE_6),
roll_limit (HEAD_MAX_CENTIDEGREE),
pitch_limit_max (PITCH_MAX_CENTIDEGREE),
pitch_limit_min (PITCH_MIN_CENTIDEGREE),
auto_trim (AUTO_TRIM),
manual_level (MANUAL_LEVEL),
switch_enable (REVERSE_SWITCH),
mix_mode (ELEVON_MIXING),
reverse_elevons (ELEVON_REVERSE),
reverse_ch1_elevon (ELEVON_CH1_REVERSE),
reverse_ch2_elevon (ELEVON_CH2_REVERSE),
num_resets (0),
log_bitmask (DEFAULT_LOG_BITMASK),
log_last_filenumber (0),
reset_switch_chan (0),
airspeed_cruise (AIRSPEED_CRUISE_CM),
min_gndspeed (MIN_GNDSPEED_CM),
ch7_option (CH7_OPTION),
pitch_trim (0),
RTL_altitude (ALT_HOLD_HOME_CM),
FBWB_min_altitude (ALT_HOLD_FBW_CM),
ground_temperature (0),
ground_pressure (0),
compass_enabled (MAGNETOMETER),
flap_1_percent (FLAP_1_PERCENT),
flap_1_speed (FLAP_1_SPEED),
flap_2_percent (FLAP_2_PERCENT),
flap_2_speed (FLAP_2_SPEED),
battery_monitoring (DISABLED),
volt_div_ratio (VOLT_DIV_RATIO),
curr_amp_per_volt (CURR_AMP_PER_VOLT),
input_voltage (INPUT_VOLTAGE),
pack_capacity (HIGH_DISCHARGE),
inverted_flight_ch (0),
// sonar_enabled (SONAR_ENABLED),
airspeed_enabled (AIRSPEED_SENSOR),
// ************ APMrover parameters ************************
// - JLN update
closed_loop_nav (CLOSED_LOOP_NAV),
auto_wp_radius (AUTO_WP_RADIUS),
nudgeoffset (NUDGE_OFFSET),
turn_gain (TURN_GAIN),
// ************************************************************
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
pidServoRoll (SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
pidServoPitch (SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM)
{}
};
#endif // PARAMETERS_H
/* ************ ThermoPilot parameters (old parameters setup ) ************************
low_rate_turn (LOW_RATE_TURN, k_param_low_rate_turn, PSTR("TP_LOWR_TURN")),
medium_rate_turn (MEDIUM_RATE_TURN, k_param_medium_rate_turn, PSTR("TP_MEDR_TURN")),
high_rate_turn (HIGH_RATE_TURN, k_param_high_rate_turn, PSTR("TP_HIGR_TURN")),
search_mode_turn (SEARCH_MODE_TURN, k_param_search_mode_turn, PSTR("TP_SRCM_TURN")),
slope_thermal (SLOPE_THERMAL, k_param_slope_thermal, PSTR("TP_SLOPE_THER")),
auto_thermal (AUTO_THERMAL, k_param_auto_thermal, PSTR("TP_AUTO_THER")),
stab_thermal (STAB_THERMAL, k_param_auto_thermal, PSTR("TP_STAB_THER")),
closed_loop_nav (CLOSED_LOOP_NAV, k_param_closed_loop_nav, PSTR("TP_CL_NAV")),
auto_wp_radius (AUTO_WP_RADIUS, k_param_closed_loop_nav, PSTR("TP_AWPR_NAV")),
min_alt (MIN_ALT, k_param_min_alt, PSTR("TP_MIN_ALT")),
max_alt (MAX_ALT, k_param_max_alt, PSTR("TP_MAX_ALT")),
max_dist (MAX_DIST, k_param_max_dist, PSTR("TP_MAX_DIST")),
sarsec_branch (SARSEC_BRANCH, k_param_sarsec_branch, PSTR("TP_SARSEC")),
************************************************************/

174
APMrover2/Parameters.pde Normal file
View File

@ -0,0 +1,174 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
ArduPlane parameter definitions
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.
*/
#define GSCALAR(v, name) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, class::var_info }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, class::var_info }
static const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(format_version, "FORMAT_VERSION"),
GSCALAR(software_type, "SYSID_SW_TYPE"),
GSCALAR(sysid_this_mav, "SYSID_THISMAV"),
GSCALAR(sysid_my_gcs, "SYSID_MYGCS"),
GSCALAR(serial3_baud, "SERIAL3_BAUD"),
GSCALAR(kff_pitch_compensation, "KFF_PTCHCOMP"),
GSCALAR(kff_rudder_mix, "KFF_RDDRMIX"),
GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR"),
GSCALAR(kff_throttle_to_pitch, "KFF_THR2PTCH"),
GSCALAR(manual_level, "MANUAL_LEVEL"),
GSCALAR(crosstrack_gain, "XTRK_GAIN_SC"),
GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD"),
GSCALAR(altitude_mix, "ALT_MIX"),
GSCALAR(airspeed_ratio, "ARSPD_RATIO"),
GSCALAR(airspeed_offset, "ARSPD_OFFSET"),
GSCALAR(command_total, "CMD_TOTAL"),
GSCALAR(command_index, "CMD_INDEX"),
GSCALAR(waypoint_radius, "WP_RADIUS"),
GSCALAR(loiter_radius, "WP_LOITER_RAD"),
#if GEOFENCE_ENABLED == ENABLED
GSCALAR(fence_action, "FENCE_ACTION"),
GSCALAR(fence_total, "FENCE_TOTAL"),
GSCALAR(fence_channel, "FENCE_CHANNEL"),
GSCALAR(fence_minalt, "FENCE_MINALT"),
GSCALAR(fence_maxalt, "FENCE_MAXALT"),
#endif
GSCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN"),
GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX"),
GSCALAR(throttle_min, "THR_MIN"),
GSCALAR(throttle_max, "THR_MAX"),
GSCALAR(throttle_slewrate, "THR_SLEWRATE"),
GSCALAR(throttle_fs_enabled, "THR_FAILSAFE"),
GSCALAR(throttle_fs_value, "THR_FS_VALUE"),
GSCALAR(throttle_cruise, "TRIM_THROTTLE"),
GSCALAR(short_fs_action, "FS_SHORT_ACTN"),
GSCALAR(long_fs_action, "FS_LONG_ACTN"),
GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL"),
GSCALAR(flight_mode_channel, "FLTMODE_CH"),
GSCALAR(flight_mode1, "FLTMODE1"),
GSCALAR(flight_mode2, "FLTMODE2"),
GSCALAR(flight_mode3, "FLTMODE3"),
GSCALAR(flight_mode4, "FLTMODE4"),
GSCALAR(flight_mode5, "FLTMODE5"),
GSCALAR(flight_mode6, "FLTMODE6"),
GSCALAR(roll_limit, "LIM_ROLL_CD"),
GSCALAR(pitch_limit_max, "LIM_PITCH_MAX"),
GSCALAR(pitch_limit_min, "LIM_PITCH_MIN"),
GSCALAR(auto_trim, "TRIM_AUTO"),
GSCALAR(switch_enable, "SWITCH_ENABLE"),
GSCALAR(mix_mode, "ELEVON_MIXING"),
GSCALAR(reverse_elevons, "ELEVON_REVERSE"),
GSCALAR(reverse_ch1_elevon, "ELEVON_CH1_REV"),
GSCALAR(reverse_ch2_elevon, "ELEVON_CH2_REV"),
GSCALAR(num_resets, "SYS_NUM_RESETS"),
GSCALAR(log_bitmask, "LOG_BITMASK"),
GSCALAR(log_last_filenumber, "LOG_LASTFILE"),
GSCALAR(reset_switch_chan, "RST_SWITCH_CH"),
GSCALAR(airspeed_cruise, "TRIM_ARSPD_CM"),
GSCALAR(min_gndspeed, "MIN_GNDSPD_CM"),
GSCALAR(ch7_option, "CH7_OPT"),
GSCALAR(pitch_trim, "TRIM_PITCH_CD"),
GSCALAR(RTL_altitude, "ALT_HOLD_RTL"),
GSCALAR(FBWB_min_altitude, "ALT_HOLD_FBWCM"),
GSCALAR(ground_temperature, "GND_TEMP"),
GSCALAR(ground_pressure, "GND_ABS_PRESS"),
GSCALAR(compass_enabled, "MAG_ENABLE"),
GSCALAR(flap_1_percent, "FLAP_1_PERCNT"),
GSCALAR(flap_1_speed, "FLAP_1_SPEED"),
GSCALAR(flap_2_percent, "FLAP_2_PERCNT"),
GSCALAR(flap_2_speed, "FLAP_2_SPEED"),
GSCALAR(battery_monitoring, "BATT_MONITOR"),
GSCALAR(volt_div_ratio, "VOLT_DIVIDER"),
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT"),
GSCALAR(input_voltage, "INPUT_VOLTS"),
GSCALAR(pack_capacity, "BATT_CAPACITY"),
GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH"),
//GSCALAR(sonar_enabled, "SONAR_ENABLE"),
GSCALAR(airspeed_enabled, "ARSPD_ENABLE"),
// ************************************************************
// APMrover parameters - JLN update
GSCALAR(closed_loop_nav, "ROV_CL_NAV"),
GSCALAR(auto_wp_radius, "ROV_AWPR_NAV"),
GSCALAR(nudgeoffset, "ROV_NUDGE"),
GSCALAR(turn_gain, "ROV_GAIN"),
// ************************************************************
GGROUP(channel_roll, "RC1_", RC_Channel),
GGROUP(channel_pitch, "RC2_", RC_Channel),
GGROUP(channel_throttle, "RC3_", RC_Channel),
GGROUP(channel_rudder, "RC4_", RC_Channel),
GGROUP(rc_5, "RC5_", RC_Channel_aux),
GGROUP(rc_6, "RC6_", RC_Channel_aux),
GGROUP(rc_7, "RC7_", RC_Channel_aux),
GGROUP(rc_8, "RC8_", RC_Channel_aux),
GGROUP(pidNavRoll, "HDNG2RLL_", PID),
GGROUP(pidServoRoll, "RLL2SRV_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID),
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidServoRudder, "YW2SRV_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
// variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
GOBJECT(imu, "IMU_", IMU)
};
static void load_parameters(void)
{
// setup the AP_Var subsystem for storage to EEPROM
if (!AP_Param::setup(var_info, sizeof(var_info)/sizeof(var_info[0]), WP_START_BYTE)) {
// this can only happen on startup, and its a definate coding
// error. Best not to continue so the programmer catches it
while (1) {
Serial.println_P(PSTR("ERROR: Failed to setup AP_Param"));
delay(1000);
}
}
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
// erase all parameters
Serial.printf_P(PSTR("Firmware change: erasing EEPROM...\n"));
AP_Param::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
}
}

85
APMrover2/command_description.txt Normal file
View File

@ -0,0 +1,85 @@
ArduPilotMega 2.0 Commands
Command Structure in bytes
0 0x00 byte Command ID
1 0x01 byte Parameter 1
2 0x02 long Parameter 2
3 0x03 ..
4 0x04 ..
5 0x05 ..
6 0x06 long Parameter 3
7 0x07 ..
8 0x08 ..
9 0x09 ..
10 0x0A long Parameter 4
11 0x0B ..
11 0x0C ..
11 0x0D ..
***********************************
Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
***********************************
Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) altitude lat lon
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
0x16 / 22 MAV_CMD_NAV_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.
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
***********************************
May Commands - these commands are optional to finish
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) -
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT - alt (finish) rate (cm/sec) -
Note: rate must be > 10 cm/sec due to integer math
MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
0x72 / 114 MAV_CMD_CONDITION_DISTANCE - - distance (meters) -
0x71 / 115 MAV_CMD_CONDITION_YAW angle speed direction (-1,1) rel (1), abs (0)
***********************************
Unexecuted commands > MAV_CMD_NAV_LAST are dropped when ready for the next command < MAV_CMD_NAV_LAST so plan/queue commands accordingly!
For example if you had a string of CMD_MAV_CONDITION commands following a 0x10 command that had not finished when the waypoint was
reached, the unexecuted CMD_MAV_CONDITION and CMD_MAV_DO commands would be skipped and the next command < MAV_CMD_NAV_LAST would be loaded
***********************************
Now Commands - these commands are executed once until no more new now commands are available
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0xB1 / 177 MAV_CMD_DO_JUMP index - repeat count -
Note: The repeat count must be greater than 1 for the command to execute. Use a repeat count of 1 if you intend a single use.
0XB2 / 178 MAV_CMD_DO_CHANGE_SPEED Speed type Speed (m/s) Throttle (Percent) -
(0=Airspeed, 1=Ground Speed) (-1 indicates no change)(-1 indicates no change)
0xB3 / 179 MAV_CMD_DO_SET_HOME Use current altitude lat lon
(1=use current location, 0=use specified location)
0xB4 / 180 MAV_CMD_DO_SET_PARAMETER Param number Param value (NOT CURRENTLY IMPLEMENTED IN APM)
0xB5 / 181 MAV_CMD_DO_SET_RELAY Relay number On/off (1/0) - -
0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY Relay number Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
0xB7 / 183 MAV_CMD_DO_SET_SERVO Servo number (5-8) On/off (1/0) - -
0xB6 / 184 MAV_CMD_DO_REPEAT_SERVO Servo number (5-8) Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event

284
APMrover2/commands.pde Normal file
View File

@ -0,0 +1,284 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* Functions in this file:
void init_commands()
void update_auto()
void reload_commands()
struct Location get_cmd_with_index(int i)
void set_cmd_with_index(struct Location temp, int i)
void increment_cmd_index()
void decrement_cmd_index()
long read_alt_to_hold()
void set_next_WP(struct Location *wp)
void set_guided_WP(void)
void init_home()
************************************************************
*/
static void init_commands()
{
g.command_index.set_and_save(0);
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
}
static void update_auto()
{
if (g.command_index >= g.command_total) {
handle_no_commands();
if(g.command_total == 0) {
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
}
} else {
if(g.command_index != 0) {
g.command_index = nav_command_index;
nav_command_index--;
}
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
process_next_command();
}
}
// Reload all the wp
static void reload_commands()
{
init_commands();
g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all?
decrement_cmd_index();
}
// Getters
// -------
static struct Location get_cmd_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 > g.command_total) {
memset(&temp, 0, sizeof(temp));
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.options = 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);
}
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
temp.alt += home.alt;
}
return temp;
}
// Setters
// -------
static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.command_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask
// XXX What is this trying to do?
if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){
temp.options = MASK_OPTIONS_RELATIVE_ALT;
} else {
temp.options = 0;
}
eeprom_write_byte((uint8_t *) mem, temp.id);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.options);
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);
}
static void increment_cmd_index()
{
if (g.command_index <= g.command_total) {
g.command_index.set_and_save(g.command_index + 1);
}
}
static void decrement_cmd_index()
{
if (g.command_index > 0) {
g.command_index.set_and_save(g.command_index - 1);
}
}
static long read_alt_to_hold()
{
if(g.RTL_altitude < 0)
return current_loc.alt;
else
return g.RTL_altitude + home.alt;
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
static void set_next_WP(struct Location *wp)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP = next_WP;
// Load the next_WP slot
// ---------------------
next_WP = *wp;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_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;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
static void set_guided_WP(void)
{
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
// Load the next_WP slot
// ---------------------
next_WP = guided_WP;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
offset_altitude = next_WP.alt - prev_WP.alt;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
// run this at setup on the ground
// -------------------------------
void init_home()
{
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
// block until we get a good fix
// -----------------------------
while (!g_gps->new_data || !g_gps->fix) {
g_gps->update();
}
home.id = MAV_CMD_NAV_WAYPOINT;
#if HIL_MODE != HIL_MODE_ATTITUDE
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
gps_base_alt = max(g_gps->altitude, 0);
home.alt = g_gps->altitude;;
// Home is always 0
#else
struct Location temp = get_cmd_with_index(0); // JLN update - for HIL test only get the home param stored in the FPL
if (temp.alt > 0) {
home.lng = temp.lng; // Lon * 10**7
home.lat = temp.lat; // Lat * 10**7
} else {
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
}
gps_base_alt = g_gps->altitude;; // get the stored home altitude as the base ref for AGL calculation.
home.alt = g_gps->altitude;;
#endif
home_is_set = true;
//gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
// Save prev loc
// -------------
next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
}

519
APMrover2/commands_logic.pde Normal file
View File

@ -0,0 +1,519 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static void
handle_process_nav_cmd()
{
// reset navigation integrators
// -------------------------
reset_I();
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
switch(next_nav_command.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp();
break;
case MAV_CMD_NAV_LAND: // LAND to Waypoint
do_land();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited();
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
do_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
do_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
default:
break;
}
}
static void
handle_process_condition_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_CONDITION_DELAY:
do_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
do_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt();
break;
default:
break;
}
}
static void handle_process_do_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_DO_JUMP:
do_jump();
break;
case MAV_CMD_DO_CHANGE_SPEED:
do_change_speed();
break;
case MAV_CMD_DO_SET_HOME:
do_set_home();
break;
case MAV_CMD_DO_SET_SERVO:
do_set_servo();
break;
case MAV_CMD_DO_SET_RELAY:
do_set_relay();
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo();
break;
case MAV_CMD_DO_REPEAT_RELAY:
do_repeat_relay();
break;
#if MOUNT == ENABLED
// Sets the region of interest (ROI) for a sensor set or the
// vehicle itself. This can then be used by the vehicles control
// system to control the vehicle attitude and the attitude of various
// devices such as cameras.
// |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
case MAV_CMD_DO_SET_ROI:
camera_mount.set_roi_cmd();
break;
case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
camera_mount.configure_cmd();
break;
case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|
camera_mount.control_cmd();
break;
#endif
}
}
static void handle_no_commands()
{
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = home;
next_nav_command.alt = read_alt_to_hold();
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
handle_process_nav_cmd();
}
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
static bool verify_nav_command() // Returns true if command complete
{
switch(nav_command_ID) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
break;
case MAV_CMD_NAV_LAND:
return verify_land();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
break;
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
return false;
break;
}
}
static bool verify_condition_command() // Returns true if command complete
{
switch(non_nav_command_ID) {
case NO_COMMAND:
break;
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
case WAIT_COMMAND:
return 0;
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
break;
}
return false;
}
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
static void do_RTL(void)
{
prev_WP = current_loc;
control_mode = RTL;
crash_timer = 0;
next_WP = home;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
}
static void do_takeoff()
{
set_next_WP(&next_nav_command);
}
static void do_nav_wp()
{
set_next_WP(&next_nav_command);
}
static void do_land()
{
set_next_WP(&next_nav_command);
}
static void do_loiter_unlimited()
{
set_next_WP(&next_nav_command);
}
static void do_loiter_turns()
{
set_next_WP(&next_nav_command);
loiter_total = next_nav_command.p1 * 360;
}
static void do_loiter_time()
{
set_next_WP(&next_nav_command);
loiter_time = millis();
loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
static bool verify_takeoff()
{ return true;
}
static bool verify_land()
{
}
static bool verify_nav_wp()
{
hold_course = -1;
update_crosstrack();
if(g.auto_wp_radius)
{ calc_turn_radius(); // JLN update - auto-adap the wp_radius Vs the gspeed and max roll angle
if ((wp_distance > 0) && (wp_distance <= wp_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
return true;
}
} else {
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
return true;
}
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
return true;
}
return false;
}
static bool verify_loiter_unlim()
{
update_loiter();
calc_bearing_error();
return false;
}
static bool verify_loiter_time()
{
update_loiter();
calc_bearing_error();
if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
return true;
}
return false;
}
static bool verify_loiter_turns()
{
update_loiter();
calc_bearing_error();
if(loiter_sum > loiter_total) {
loiter_total = 0;
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
// clear the command queue;
return true;
}
return false;
}
static bool verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
return true;
}else{
return false;
}
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/
static void do_wait_delay()
{
condition_start = millis();
condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
}
static void do_change_alt()
{
condition_rate = abs((int)next_nonnav_command.lat);
condition_value = next_nonnav_command.alt;
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations
offset_altitude = 0; // For future nav calculations
}
static void do_within_distance()
{
condition_value = next_nonnav_command.lat;
}
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
static bool verify_wait_delay()
{
if ((unsigned)(millis() - condition_start) > condition_value){
condition_value = 0;
return true;
}
return false;
}
static bool verify_change_alt()
{
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
condition_value = 0;
return true;
}
target_altitude += condition_rate / 10;
return false;
}
static bool verify_within_distance()
{
if (wp_distance < condition_value){
condition_value = 0;
return true;
}
return false;
}
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
static void do_loiter_at_location()
{
next_WP = current_loc;
}
static void do_jump()
{
struct Location temp;
gcs_send_text_fmt(PSTR("In jump. Jumps left: %i"),next_nonnav_command.lat);
if(next_nonnav_command.lat > 0) {
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
temp = get_cmd_with_index(g.command_index);
temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter
set_cmd_with_index(temp, g.command_index);
gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
nav_command_index = next_nonnav_command.p1 - 1;
next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
process_next_command();
} else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
gcs_send_text_fmt(PSTR("setting command index: %i"),next_nonnav_command.p1 - 1);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
nav_command_index = next_nonnav_command.p1 - 1;
next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump
process_next_command();
}
}
static void do_change_speed()
{
switch (next_nonnav_command.p1)
{
case 0: // Airspeed
if(next_nonnav_command.alt > 0)
g.airspeed_cruise.set(next_nonnav_command.alt * 100);
break;
case 1: // Ground speed
g.min_gndspeed.set(next_nonnav_command.alt * 100);
break;
}
if(next_nonnav_command.lat > 0)
g.throttle_cruise.set(next_nonnav_command.lat);
}
static void do_set_home()
{
if(next_nonnav_command.p1 == 1 && GPS_enabled) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_nonnav_command.lng; // Lon * 10**7
home.lat = next_nonnav_command.lat; // Lat * 10**7
home.alt = max(next_nonnav_command.alt, 0);
home_is_set = true;
}
}
static void do_set_servo()
{
APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
static void do_set_relay()
{
if (next_nonnav_command.p1 == 1) {
relay.on();
} else if (next_nonnav_command.p1 == 0) {
relay.off();
}else{
relay.toggle();
}
}
static void do_repeat_servo()
{
event_id = next_nonnav_command.p1 - 1;
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.lat * 2;
event_value = next_nonnav_command.alt;
switch(next_nonnav_command.p1) {
case CH_5:
event_undo_value = g.rc_5.radio_trim;
break;
case CH_6:
event_undo_value = g.rc_6.radio_trim;
break;
case CH_7:
event_undo_value = g.rc_7.radio_trim;
break;
case CH_8:
event_undo_value = g.rc_8.radio_trim;
break;
}
update_events();
}
}
static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.alt * 2;
update_events();
}

146
APMrover2/commands_process.pde Normal file
View File

@ -0,0 +1,146 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t cmd_index)
{
struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index);
update_commands();
}
}
// called by 10 Hz loop
// --------------------
static void update_commands(void)
{
if(control_mode == AUTO){
if(home_is_set == true && g.command_total > 1){
process_next_command();
}
} // Other (eg GCS_Auto) modes may be implemented here
}
static void verify_commands(void)
{
if(verify_nav_command()){
nav_command_ID = NO_COMMAND;
}
if(verify_condition_command()){
non_nav_command_ID = NO_COMMAND;
}
}
static void process_next_command()
{
// This function makes sure that we always have a current navigation command
// and loads conditional or immediate commands if applicable
struct Location temp;
byte old_index = 0;
// these are Navigation/Must commands
// ---------------------------------
if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
old_index = nav_command_index;
temp.id = MAV_CMD_NAV_LAST;
while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
nav_command_index++;
temp = get_cmd_with_index(nav_command_index);
}
gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
if(nav_command_index > g.command_total){
// we are out of commands!
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
if (g.closed_loop_nav) { // JLN update - replay the FPL (CLOSED_LOOP_NAV)
change_command(1);
return;
} else {
handle_no_commands();
}
} else {
next_nav_command = temp;
nav_command_ID = next_nav_command.id;
non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded
non_nav_command_ID = NO_COMMAND;
process_nav_cmd();
}
}
// these are Condition/May and Do/Now commands
// -------------------------------------------
if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command
non_nav_command_index = old_index + 1;
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
} else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
non_nav_command_index++;
}
//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
temp = get_cmd_with_index(non_nav_command_index);
if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do
g.command_index.set_and_save(nav_command_index);
non_nav_command_index = nav_command_index;
non_nav_command_ID = WAIT_COMMAND;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
} else { // The next command is a non-nav command. Prepare to execute it.
g.command_index.set_and_save(non_nav_command_index);
next_nonnav_command = temp;
non_nav_command_ID = next_nonnav_command.id;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
process_non_nav_command();
}
}
}
/**************************************************/
// These functions implement the commands.
/**************************************************/
static void process_nav_cmd()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
// clear non-nav command ID and index
non_nav_command_index = NO_COMMAND; // Redundant - remove?
non_nav_command_ID = NO_COMMAND; // Redundant - remove?
handle_process_nav_cmd();
}
static void process_non_nav_command()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_condition_command();
} else {
handle_process_do_command();
// flag command ID so a new one is loaded
// -----------------------------------------
non_nav_command_ID = NO_COMMAND;
}
}

873
APMrover2/config.h Normal file
View File

@ -0,0 +1,873 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// 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
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// APM HARDWARE
//
#ifndef CONFIG_APM_HARDWARE
# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
#endif
#if defined( __AVR_ATmega1280__ )
#define CLI_ENABLED DISABLED
#define LOGGING_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// APM2 HARDWARE DEFAULTS
//
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define CONFIG_IMU_TYPE CONFIG_IMU_MPU6000
# define CONFIG_PUSHBUTTON DISABLED
# define CONFIG_RELAY DISABLED
# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
# define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN
# define MAGNETOMETER ENABLED
# ifdef APM2_BETA_HARDWARE
# define CONFIG_BARO AP_BARO_BMP085
# else // APM2 Production Hardware (default)
# define CONFIG_BARO AP_BARO_MS5611
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// LED and IO Pins
//
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
# define A_LED_PIN 37
# define B_LED_PIN 36
# define C_LED_PIN 35
# define LED_ON HIGH
# define LED_OFF LOW
# define SLIDE_SWITCH_PIN 40
# define PUSHBUTTON_PIN 41
# define USB_MUX_PIN -1
# define CONFIG_RELAY ENABLED
# define BATTERY_PIN_1 0
# define CURRENT_PIN_1 1
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
# define LED_ON LOW
# define LED_OFF HIGH
# define SLIDE_SWITCH_PIN (-1)
# define PUSHBUTTON_PIN (-1)
# define CLI_SLIDER_ENABLED DISABLED
# define USB_MUX_PIN 23
# define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2
#endif
//////////////////////////////////////////////////////////////////////////////
// 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
//////////////////////////////////////////////////////////////////////////////
// IMU Selection
//
#ifndef CONFIG_IMU_TYPE
# define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN
#endif
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
# ifndef CONFIG_MPU6000_CHIP_SELECT_PIN
# define CONFIG_MPU6000_CHIP_SELECT_PIN 53
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// ADC Enable - used to eliminate for systems which don't have ADC.
//
#ifndef CONFIG_ADC
# if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
# define CONFIG_ADC ENABLED
# else
# define CONFIG_ADC DISABLED
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// Barometer
//
#ifndef CONFIG_BARO
# define CONFIG_BARO AP_BARO_BMP085
#endif
//////////////////////////////////////////////////////////////////////////////
// Pitot
//
#ifndef PITOT_ENABLED
# define PITOT_ENABLED DISABLED
#endif
#ifndef CONFIG_PITOT_SOURCE
# define CONFIG_PITOT_SOURCE PITOT_SOURCE_ADC
#endif
#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
# ifndef CONFIG_PITOT_SOURCE_ADC_CHANNEL
# define CONFIG_PITOT_SOURCE_ADC_CHANNEL 7
# endif
#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
# ifndef CONFIG_PITOT_SOURCE_ANALOG_PIN
# define CONFIG_PITOT_SOURCE_ANALOG_PIN 0
# endif
#else
# warning Invalid value for CONFIG_PITOT_SOURCE, disabling airspeed
# undef PITOT_ENABLED
# define PITOT_ENABLED DISABLED
#endif
#ifndef SONAR_TYPE
# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
#endif
#ifndef SONAR_ENABLED
#define SONAR_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
#ifndef SERIAL0_BAUD
# define SERIAL0_BAUD 115200
#endif
#ifndef SERIAL3_BAUD
# define SERIAL3_BAUD 57600
#endif
#ifndef CH7_OPTION
# define CH7_OPTION CH7_DO_NOTHING
#endif
#ifndef TUNING_OPTION
# define TUNING_OPTION TUN_NONE
#endif
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef BATTERY_EVENT
# define BATTERY_EVENT DISABLED
#endif
#ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 9.6
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.56 // This is the proper value for an on-board APM1 voltage divider with a 3.9kOhm resistor
//# define VOLT_DIV_RATIO 15.70 // This is the proper value for the AttoPilot 50V/90A sensor
//# define VOLT_DIV_RATIO 4.127 // This is the proper value for the AttoPilot 13.6V/45A sensor
#endif
#ifndef CURR_AMP_PER_VOLT
# define CURR_AMP_PER_VOLT 27.32 // This is the proper value for the AttoPilot 50V/90A sensor
//# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor
#endif
#ifndef CURR_AMPS_OFFSET
# define CURR_AMPS_OFFSET 0.0
#endif
#ifndef HIGH_DISCHARGE
# define HIGH_DISCHARGE 1760
#endif
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE
//
#ifndef INPUT_VOLTAGE
# define INPUT_VOLTAGE 4.68 // 4.68 is the average value for a sample set. This is the value at the processor with 5.02 applied at the servo rail
#endif
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGNETOMETER
# define MAGNETOMETER DISABLED
#endif
#ifndef MAG_ORIENTATION
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Radio channel limits
//
// Note that these are not called out in APM_Config.h.reference.
//
#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
#ifndef FLAP_1_PERCENT
# define FLAP_1_PERCENT 0
#endif
#ifndef FLAP_1_SPEED
# define FLAP_1_SPEED 255
#endif
#ifndef FLAP_2_PERCENT
# define FLAP_2_PERCENT 0
#endif
#ifndef FLAP_2_SPEED
# define FLAP_2_SPEED 255
#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 STABILIZE
#endif
#if !defined(FLIGHT_MODE_2)
# define FLIGHT_MODE_2 STABILIZE
#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 STABILIZE
#endif
#if !defined(FLIGHT_MODE_6)
# define FLIGHT_MODE_6 MANUAL
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE
// THROTTLE_FS_VALUE
// SHORT_FAILSAFE_ACTION
// LONG_FAILSAFE_ACTION
// GCS_HEARTBEAT_FAILSAFE
//
#ifndef THROTTLE_FAILSAFE
# define THROTTLE_FAILSAFE ENABLED
#endif
#ifndef THROTTLE_FS_VALUE
# define THROTTLE_FS_VALUE 950
#endif
#ifndef SHORT_FAILSAFE_ACTION
# define SHORT_FAILSAFE_ACTION 0
#endif
#ifndef LONG_FAILSAFE_ACTION
# define LONG_FAILSAFE_ACTION 0
#endif
#ifndef GCS_HEARTBEAT_FAILSAFE
# define GCS_HEARTBEAT_FAILSAFE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// AUTO_TRIM
//
#ifndef AUTO_TRIM
# define AUTO_TRIM DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// MANUAL_LEVEL
//
#ifndef MANUAL_LEVEL
# define MANUAL_LEVEL DISABLED
#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
//////////////////////////////////////////////////////////////////////////////
// ENABLE ELEVON_MIXING
//
#ifndef ELEVON_MIXING
# define ELEVON_MIXING DISABLED
#endif
#ifndef ELEVON_REVERSE
# define ELEVON_REVERSE DISABLED
#endif
#ifndef ELEVON_CH1_REVERSE
# define ELEVON_CH1_REVERSE DISABLED
#endif
#ifndef ELEVON_CH2_REVERSE
# define ELEVON_CH2_REVERSE DISABLED
#endif
#ifndef FLAPERON
# define FLAPERON DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// MOUNT (ANTENNA OR CAMERA)
//
#ifndef MOUNT
# define MOUNT DISABLED
#endif
#if defined( __AVR_ATmega1280__ ) && CAMERA == ENABLED
// The small ATmega1280 chip does not have enough memory for camera support
// so disable CLI, this will allow camera support and other improvements to fit.
// This should almost have no side effects, because the APM planner can now do a complete board setup.
#define CLI_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Altitude measurement and control.
//
#ifndef ALT_EST_GAIN
# define ALT_EST_GAIN 0.01
#endif
#ifndef ALTITUDE_MIX
# define ALTITUDE_MIX 1
#endif
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_CRUISE
//
#ifndef AIRSPEED_CRUISE
# define AIRSPEED_CRUISE 3 // 12 m/s
#endif
#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100
#ifndef GSBOOST
# define GSBOOST 0
#endif
#ifndef GSBOOST
# define GSBOOST 0
#endif
#ifndef NUDGE_OFFSET
# define NUDGE_OFFSET 0
#endif
#ifndef E_GLIDER
# define E_GLIDER ENABLED
#endif
#ifndef TURN_GAIN
# define TURN_GAIN 5
#endif
//////////////////////////////////////////////////////////////////////////////
// MIN_GNDSPEED
//
#ifndef MIN_GNDSPEED
# define MIN_GNDSPEED 0 // m/s (0 disables)
#endif
#define MIN_GNDSPEED_CM MIN_GNDSPEED*100
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control
//
#ifndef AIRSPEED_FBW_MIN
# define AIRSPEED_FBW_MIN 6
#endif
#ifndef AIRSPEED_FBW_MAX
# define AIRSPEED_FBW_MAX 22
#endif
#ifndef ALT_HOLD_FBW
# define ALT_HOLD_FBW 0
#endif
#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100
/* The following parmaeters have no corresponding control implementation
#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
#define THROTTLE_ALT_INT_MAX_CM THROTTLE_ALT_INT_MAX*100
*/
//////////////////////////////////////////////////////////////////////////////
// Servo Mapping
//
#ifndef THROTTLE_MIN
# define THROTTLE_MIN 0 // percent
#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 45
#endif
#ifndef PITCH_MAX
# define PITCH_MAX 15
#endif
#ifndef PITCH_MIN
# define PITCH_MIN -25
#endif
#define HEAD_MAX_CENTIDEGREE HEAD_MAX * 100
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
#define PITCH_MIN_CENTIDEGREE PITCH_MIN * 100
//////////////////////////////////////////////////////////////////////////////
// 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
#define SERVO_ROLL_INT_MAX_CENTIDEGREE SERVO_ROLL_INT_MAX*100
#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
#define SERVO_PITCH_INT_MAX_CENTIDEGREE SERVO_PITCH_INT_MAX*100
#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 0
#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
#define NAV_ROLL_INT_MAX_CENTIDEGREE NAV_ROLL_INT_MAX*100
#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
#define NAV_PITCH_ASP_INT_MAX_CMSEC NAV_PITCH_ASP_INT_MAX*100
#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
#define NAV_PITCH_ALT_INT_MAX_CM NAV_PITCH_ALT_INT_MAX*100
//////////////////////////////////////////////////////////////////////////////
// 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 0
#endif
#ifndef T_TO_P
# define T_TO_P 0
#endif
#ifndef PITCH_TARGET
# define PITCH_TARGET 0
#endif
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 1 // deg/m
#endif
#ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 20 // deg
#endif
# define XTRACK_GAIN_SCALED XTRACK_GAIN*100
# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE*100
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUGGING
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
#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 DISABLED
#endif
#ifndef LOG_CTUN
# define LOG_CTUN ENABLED
#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 LOG_CUR
# define LOG_CUR DISABLED
#endif
// calculate the default log_bitmask
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
#define DEFAULT_LOG_BITMASK \
LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(RAW) | \
LOGBIT(CMD) | \
LOGBIT(CUR)
//////////////////////////////////////////////////////////////////////////////
// Navigation defaults
//
#ifndef WP_RADIUS_DEFAULT
# define WP_RADIUS_DEFAULT 30
#endif
#ifndef LOITER_RADIUS_DEFAULT
# define LOITER_RADIUS_DEFAULT 60
#endif
#ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 100
#endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
#ifndef USE_CURRENT_ALT
# define USE_CURRENT_ALT FALSE
#endif
#ifndef INVERTED_FLIGHT_PWM
# define INVERTED_FLIGHT_PWM 1750
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
#ifndef STANDARD_SPEED
# define STANDARD_SPEED 3.0
#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
#endif
#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
// use this to enable servos in HIL mode
#ifndef HIL_SERVOS
# define HIL_SERVOS DISABLED
#endif
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED
#endif
// use this to disable the CLI slider switch
#ifndef CLI_SLIDER_ENABLED
# define CLI_SLIDER_ENABLED ENABLED
#endif
// delay to prevent Xbee bricking, in milliseconds
#ifndef MAVLINK_TELEMETRY_PORT_DELAY
# define MAVLINK_TELEMETRY_PORT_DELAY 2000
#endif
// use this to disable gen-fencing
#ifndef GEOFENCE_ENABLED
# define GEOFENCE_ENABLED ENABLED
#endif
// pwm value on FENCE_CHANNEL to use to enable fenced mode
#ifndef FENCE_ENABLE_PWM
# define FENCE_ENABLE_PWM 1750
#endif
// a digital pin to set high when the geo-fence triggers. Defaults
// to -1, which means don't activate a pin
#ifndef FENCE_TRIGGERED_PIN
# define FENCE_TRIGGERED_PIN -1
#endif
// if RESET_SWITCH_CH is not zero, then this is the PWM value on
// that channel where we reset the control mode to the current switch
// position (to for example return to switched mode after failsafe or
// fence breach)
#ifndef RESET_SWITCH_CHAN_PWM
# define RESET_SWITCH_CHAN_PWM 1750
#endif
// experimental quaternion code
#ifndef QUATERNION_ENABLE
# define QUATERNION_ENABLE DISABLED
#endif

147
APMrover2/control_modes.pde Normal file
View File

@ -0,0 +1,147 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void read_control_switch()
{
byte switchPosition = readSwitch();
// If switchPosition = 255 this indicates that the mode control channel input was out of range
// If we get this value we do not want to change modes.
if(switchPosition == 255) return;
// we look for changes in the switch position. If the
// RST_SWITCH_CH parameter is set, then it is a switch that can be
// used to force re-reading of the control switch. This is useful
// when returning to the previous mode after a failsafe or fence
// breach. This channel is best used on a momentary switch (such
// as a spring loaded trainer switch).
if (oldSwitchPosition != switchPosition ||
(g.reset_switch_chan != 0 &&
APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
set_mode(flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
prev_WP = current_loc;
// reset navigation integrators
// -------------------------
reset_I();
}
}
static byte readSwitch(void){
uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition
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;
}
static void reset_control_switch()
{
oldSwitchPosition = 0;
read_control_switch();
}
#define CH_7_PWM_TRIGGER 1800
// read at 10 hz
// set this to your trainer switch
static void read_trim_switch()
{
if (g.ch7_option == CH7_SAVE_WP){ // set to 1
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
trim_flag = false;
if(control_mode == MANUAL){ // if SW7 is ON in MANUAL = Erase the Flight Plan
// reset the mission
CH7_wp_index = 0;
g.command_total.set_and_save(CH7_wp_index);
#if X_PLANE == ENABLED
Serial.printf_P(PSTR("*** RESET the FPL\n"));
#endif
CH7_wp_index = 1;
return;
} else if (control_mode == STABILIZE) { // if SW7 is ON in STABILIZE = record the Wp
// set the next_WP (home is stored at 0)
// max out at 100 since I think we need to stay under the EEPROM limit
CH7_wp_index = constrain(CH7_wp_index, 1, 100);
current_loc.id = MAV_CMD_NAV_WAYPOINT;
// store the index
g.command_total.set_and_save(CH7_wp_index);
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
#if X_PLANE == ENABLED
Serial.printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index);
#endif
// increment index
CH7_wp_index++;
} else if (control_mode == AUTO) { // if SW7 is ON in AUTO = set to RTL
set_mode(RTL);
}
}
}
}
else if (g.ch7_option == CH7_RTL){ // set to 6
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
trim_flag = false;
if (control_mode == STABILIZE) {
set_mode(RTL);
}
}
}
}
}
static void update_servo_switches()
{
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
#if HIL_MODE != HIL_MODE_ATTITUDE // JLN update
if (!g.switch_enable) {
// switches are disabled in EEPROM (see SWITCH_ENABLE option)
// this means the EEPROM control of all channel reversal is enabled
return;
}
// up is reverse
// up is elevon
g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode
if (g.mix_mode == 0) {
g.channel_roll.set_reverse((PINE & 128) ? true : false);
g.channel_pitch.set_reverse((PINE & 64) ? true : false);
g.channel_rudder.set_reverse((PINL & 64) ? true : false);
} else {
g.reverse_elevons = (PINE & 128) ? true : false;
g.reverse_ch1_elevon = (PINE & 64) ? true : false;
g.reverse_ch2_elevon = (PINL & 64) ? true : false;
}
#else
g.mix_mode = 0; // 1 for elevon mode // setup for the the Predator MQ9 of AeroSim - JLN update
g.channel_roll.set_reverse(false); // roll reversed
g.channel_pitch.set_reverse(false); // pitch normal
g.channel_rudder.set_reverse(false); // yaw normal
#endif
#endif
}

68
APMrover2/createTags Normal file
View File

@ -0,0 +1,68 @@
#!/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

264
APMrover2/defines.h Normal file
View File

@ -0,0 +1,264 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef _DEFINES_H
#define _DEFINES_H
// Internal defines, don't edit and expect things to work
// -------------------------------------------------------
#define TRUE 1
#define FALSE 0
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
#define DEBUG 0
#define LOITER_RANGE 60 // for calculating power outside of loiter radius
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
// failsafe
// ----------------------
#define FAILSAFE_NONE 0
#define FAILSAFE_SHORT 1
#define FAILSAFE_LONG 2
#define FAILSAFE_GCS 3
#define FAILSAFE_SHORT_TIME 1500 // Miliiseconds
#define FAILSAFE_LONG_TIME 20000 // Miliiseconds
// active altitude sensor
// ----------------------
#define SONAR 0
#define BARO 1
// CH 7 control
#define CH7_DO_NOTHING 0
#define CH7_SAVE_WP 1
#define CH7_LEO 2
#define CH7_THERMAL 3
#define CH7_SARSEC 4
#define CH7_SARGRID 5
#define CH7_RTL 6
#define CH7_TUNING 7
// CH_7 Tuning
// -----------
#define TUN_NONE 0
// Attitude
#define TUN_STABILIZE_KP 1
#define TUN_STABILIZE_KI 2
#define TUN_STABILIZE_KD 3
#define TUN_YAW_KP 4
#define TUN_YAW_KI 5
#define TUN_YAW_KD 6
#define TUN_STABROLL_KP 7
#define TUN_STABROLL_KI 8
#define TUN_STABROLL_KD 9
#define TUN_STABPITCH_KP 10
#define TUN_STABPITCH_KI 11
#define TUN_STABPITCH_KD 12
#define PITOT_SOURCE_ADC 1
#define PITOT_SOURCE_ANALOG_PIN 2
#define T6 1000000
#define T7 10000000
// 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_MTK16 6
#define GPS_PROTOCOL_AUTO 7
#define CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_THROTTLE CH_3
#define CH_RUDDER CH_4
#define CH_YAW CH_4
#define CH_AIL1 CH_5
#define CH_AIL2 CH_6
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 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
#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed
// Fly By Wire B and Fly By Wire C require airspeed sensor
#define AUTO 10
#define RTL 11
#define LOITER 12
//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM
//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM
#define GUIDED 15
#define INITIALISING 16 // in startup routines
#define HEADALT 17 // Lock the current heading and altitude
#define SARSEC 18 // Run a SAR type Sector Pattern
#define SARGRID 19 // Run a SAR type Grid Pattern
#define THERMAL 20 // Thermal hunter mode
#define LAND 21 // Landing mode
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0
#define WAIT_COMMAND 255
// Command/Waypoint/Location Options Bitmask
//--------------------
#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative altitude
//repeating events
#define NO_REPEAT 0
#define CH_5_TOGGLE 1
#define CH_6_TOGGLE 2
#define CH_7_TOGGLE 3
#define CH_8_TOGGLE 4
#define RELAY_TOGGLE 5
#define STOP_REPEAT 10
#define MAV_CMD_CONDITION_YAW 23
// GCS Message ID's
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
/// create new MSG_ IDs for additional messages on the same
/// stream
enum ap_message {
MSG_HEARTBEAT,
MSG_ATTITUDE,
MSG_LOCATION,
MSG_EXTENDED_STATUS1,
MSG_EXTENDED_STATUS2,
MSG_NAV_CONTROLLER_OUTPUT,
MSG_CURRENT_WAYPOINT,
MSG_VFR_HUD,
MSG_RADIO_OUT,
MSG_RADIO_IN,
MSG_RAW_IMU1,
MSG_RAW_IMU2,
MSG_RAW_IMU3,
MSG_GPS_STATUS,
MSG_GPS_RAW,
MSG_SERVO_OUT,
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
MSG_STATUSTEXT,
MSG_FENCE_STATUS,
MSG_RETRY_DEFERRED // this must be last
};
enum gcs_severity {
SEVERITY_LOW=1,
SEVERITY_MEDIUM,
SEVERITY_HIGH,
SEVERITY_CRITICAL
};
// Logging parameters
#define LOG_INDEX_MSG 0xF0
#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_CURRENT_MSG 0x09
#define LOG_STARTUP_MSG 0x0A
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
#define MAX_NUM_LOGS 100
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2)
#define MASK_LOG_PM (1<<3)
#define MASK_LOG_CTUN (1<<4)
#define MASK_LOG_NTUN (1<<5)
#define MASK_LOG_MODE (1<<6)
#define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
// 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_COMMAND_INDEX 2
#define EVENT_LOADED_WAYPOINT 3
#define EVENT_LOOP 4
// Climb rate calculations
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
#define RELAY_PIN 47
// sonar
#define MAX_SONAR_XL 0
#define MAX_SONAR_LV 1
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
#define AN4 4
#define AN5 5
#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
// fence points are stored at the end of the EEPROM
#define MAX_FENCEPOINTS 20
#define FENCE_WP_SIZE sizeof(Vector2l)
#define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE))
#define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
#define ONBOARD_PARAM_NAME_LENGTH 15
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)
// mark a function as not to be inlined
#define NOINLINE __attribute__((noinline))
#define CONFIG_IMU_OILPAN 1
#define CONFIG_IMU_MPU6000 2
#define APM_HARDWARE_APM1 1
#define APM_HARDWARE_APM2 2
#define AP_BARO_BMP085 1
#define AP_BARO_MS5611 2
#endif // _DEFINES_H

114
APMrover2/events.pde Normal file
View File

@ -0,0 +1,114 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void failsafe_short_on_event(int fstype)
{
// This is how to handle a short loss of control signal failsafe.
failsafe = fstype;
ch3_failsafe_timer = millis();
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
set_mode(CIRCLE);
break;
case AUTO:
case GUIDED:
case LOITER:
if(g.short_fs_action == 1) {
set_mode(RTL);
}
break;
case CIRCLE:
case RTL:
default:
break;
}
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
static void failsafe_long_on_event(int fstype)
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC
failsafe = fstype;
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
case CIRCLE:
set_mode(RTL);
break;
case AUTO:
case GUIDED:
case LOITER:
if(g.long_fs_action == 1) {
set_mode(RTL);
}
break;
case RTL:
default:
break;
}
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
static void failsafe_short_off_event()
{
// We're back in radio contact
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
failsafe = FAILSAFE_NONE;
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
reset_control_switch();
// Reset control integrators
// ---------------------
reset_I();
}
#if BATTERY_EVENT == ENABLED
static void low_battery_event(void)
{
gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
}
#endif
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if (event_repeat > 0){
event_repeat --;
}
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
event_timer = millis();
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
APM_RC.OutputCh(event_id, event_value); // send to Servos
} else {
APM_RC.OutputCh(event_id, event_undo_value);
}
}
if (event_id == RELAY_TOGGLE) {
relay.toggle();
}
}
}

46
APMrover2/failsafe.pde Normal file
View File

@ -0,0 +1,46 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
failsafe support
Andrew Tridgell, December 2011
*/
/*
our failsafe strategy is to detect main loop lockup and switch to
passing inputs straight from the RC inputs to RC outputs.
*/
/*
this failsafe_check function is called from the core timer interrupt
at 1kHz.
*/
void failsafe_check(uint32_t tnow)
{
static uint16_t last_mainLoop_count;
static uint32_t last_timestamp;
static bool in_failsafe;
if (mainLoop_count != last_mainLoop_count) {
// the main loop is running, all is OK
last_mainLoop_count = mainLoop_count;
last_timestamp = tnow;
in_failsafe = false;
return;
}
if (tnow - last_timestamp > 200000) {
// we have gone at least 0.2 seconds since the main loop
// ran. That means we're in trouble, or perhaps are in
// an initialisation routine or log erase. Start passing RC
// inputs through to outputs
in_failsafe = true;
}
if (in_failsafe && tnow - last_timestamp > 20000) {
// pass RC inputs to outputs every 20ms
last_timestamp = tnow;
APM_RC.clearOverride();
for (uint8_t ch=0; ch<8; ch++) {
APM_RC.OutputCh(ch, APM_RC.InputCh(ch));
}
}
}

325
APMrover2/geofence.pde Normal file
View File

@ -0,0 +1,325 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
geo-fencing support
Andrew Tridgell, December 2011
*/
#if GEOFENCE_ENABLED == ENABLED
/*
The state of geo-fencing. This structure is dynamically allocated
the first time it is used. This means we only pay for the pointer
and not the structure on systems where geo-fencing is not being
used.
We store a copy of the boundary in memory as we need to access it
very quickly at runtime
*/
static struct geofence_state {
uint8_t num_points;
bool boundary_uptodate;
bool fence_triggered;
uint16_t breach_count;
uint8_t breach_type;
uint32_t breach_time;
byte old_switch_position;
/* point 0 is the return point */
Vector2l boundary[MAX_FENCEPOINTS];
} *geofence_state;
/*
fence boundaries fetch/store
*/
static Vector2l get_fence_point_with_index(unsigned i)
{
uint32_t mem;
Vector2l ret;
if (i > (unsigned)g.fence_total) {
return Vector2l(0,0);
}
// read fence point
mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
ret.x = eeprom_read_dword((uint32_t *)mem);
mem += sizeof(uint32_t);
ret.y = eeprom_read_dword((uint32_t *)mem);
return ret;
}
// save a fence point
static void set_fence_point_with_index(Vector2l &point, unsigned i)
{
uint32_t mem;
if (i >= (unsigned)g.fence_total.get()) {
// not allowed
return;
}
mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
eeprom_write_dword((uint32_t *)mem, point.x);
mem += sizeof(uint32_t);
eeprom_write_dword((uint32_t *)mem, point.y);
if (geofence_state != NULL) {
geofence_state->boundary_uptodate = false;
}
}
/*
allocate and fill the geofence state structure
*/
static void geofence_load(void)
{
uint8_t i;
if (geofence_state == NULL) {
if (memcheck_available_memory() < 512 + sizeof(struct geofence_state)) {
// too risky to enable as we could run out of stack
goto failed;
}
geofence_state = (struct geofence_state *)calloc(1, sizeof(struct geofence_state));
if (geofence_state == NULL) {
// not much we can do here except disable it
goto failed;
}
}
for (i=0; i<g.fence_total; i++) {
geofence_state->boundary[i] = get_fence_point_with_index(i);
}
geofence_state->num_points = i;
if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) {
// first point and last point must be the same
goto failed;
}
if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) {
// return point needs to be inside the fence
goto failed;
}
geofence_state->boundary_uptodate = true;
geofence_state->fence_triggered = false;
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence loaded"));
gcs_send_message(MSG_FENCE_STATUS);
return;
failed:
g.fence_action.set(FENCE_ACTION_NONE);
gcs_send_text_P(SEVERITY_HIGH,PSTR("geo-fence setup error"));
}
/*
return true if geo-fencing is enabled
*/
static bool geofence_enabled(void)
{
if (g.fence_action == FENCE_ACTION_NONE ||
g.fence_channel == 0 ||
APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM) {
// geo-fencing is disabled
if (geofence_state != NULL) {
// re-arm for when the channel trigger is switched on
geofence_state->fence_triggered = false;
}
return false;
}
if (!g_gps->fix) {
// we can't do much without a GPS fix
return false;
}
return true;
}
/*
return true if we have breached the geo-fence minimum altiude
*/
static bool geofence_check_minalt(void)
{
if (g.fence_maxalt <= g.fence_minalt) {
return false;
}
if (g.fence_minalt == 0) {
return false;
}
return (current_loc.alt < (g.fence_minalt*100) + home.alt);
}
/*
return true if we have breached the geo-fence maximum altiude
*/
static bool geofence_check_maxalt(void)
{
if (g.fence_maxalt <= g.fence_minalt) {
return false;
}
if (g.fence_maxalt == 0) {
return false;
}
return (current_loc.alt > (g.fence_maxalt*100) + home.alt);
}
/*
check if we have breached the geo-fence
*/
static void geofence_check(bool altitude_check_only)
{
if (!geofence_enabled()) {
// switch back to the chosen control mode if still in
// GUIDED to the return point
if (geofence_state != NULL &&
g.fence_action == FENCE_ACTION_GUIDED &&
g.fence_channel != 0 &&
control_mode == GUIDED &&
g.fence_total >= 5 &&
geofence_state->boundary_uptodate &&
geofence_state->old_switch_position == oldSwitchPosition &&
guided_WP.lat == geofence_state->boundary[0].x &&
guided_WP.lng == geofence_state->boundary[0].y) {
geofence_state->old_switch_position = 0;
reset_control_switch();
}
return;
}
/* allocate the geo-fence state if need be */
if (geofence_state == NULL || !geofence_state->boundary_uptodate) {
geofence_load();
if (!geofence_enabled()) {
// may have been disabled by load
return;
}
}
bool outside = false;
uint8_t breach_type = FENCE_BREACH_NONE;
if (geofence_check_minalt()) {
outside = true;
breach_type = FENCE_BREACH_MINALT;
} else if (geofence_check_maxalt()) {
outside = true;
breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only) {
Vector2l location;
location.x = current_loc.lat;
location.y = current_loc.lng;
outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) {
breach_type = FENCE_BREACH_BOUNDARY;
}
}
if (!outside) {
if (geofence_state->fence_triggered && !altitude_check_only) {
// we have moved back inside the fence
geofence_state->fence_triggered = false;
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence OK"));
#if FENCE_TRIGGERED_PIN > 0
digitalWrite(FENCE_TRIGGERED_PIN, LOW);
#endif
gcs_send_message(MSG_FENCE_STATUS);
}
// we're inside, all is good with the world
return;
}
// we are outside the fence
if (geofence_state->fence_triggered && control_mode == GUIDED) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;
}
// we are outside, and have not previously triggered.
geofence_state->fence_triggered = true;
geofence_state->breach_count++;
geofence_state->breach_time = millis();
geofence_state->breach_type = breach_type;
#if FENCE_TRIGGERED_PIN > 0
digitalWrite(FENCE_TRIGGERED_PIN, HIGH);
#endif
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered"));
gcs_send_message(MSG_FENCE_STATUS);
// see what action the user wants
switch (g.fence_action) {
case FENCE_ACTION_GUIDED:
// fly to the return point, with an altitude half way between
// min and max
if (g.fence_minalt >= g.fence_maxalt) {
// invalid min/max, use RTL_altitude
guided_WP.alt = home.alt + (g.RTL_altitude * 100);
} else {
guided_WP.alt = home.alt + 100*(g.fence_minalt + g.fence_maxalt)/2;
}
guided_WP.id = 0;
guided_WP.p1 = 0;
guided_WP.options = 0;
guided_WP.lat = geofence_state->boundary[0].x;
guided_WP.lng = geofence_state->boundary[0].y;
geofence_state->old_switch_position = oldSwitchPosition;
if (control_mode == MANUAL && g.auto_trim) {
// make sure we don't auto trim the surfaces on this change
control_mode = STABILIZE;
}
set_mode(GUIDED);
break;
}
}
/*
return true if geofencing allows stick mixing. When we have
triggered failsafe and are in GUIDED mode then stick mixing is
disabled. Otherwise the aircraft may not be able to recover from
a breach of the geo-fence
*/
static bool geofence_stickmixing(void) {
if (geofence_enabled() &&
geofence_state != NULL &&
geofence_state->fence_triggered &&
control_mode == GUIDED) {
// don't mix in user input
return false;
}
// normal mixing rules
return true;
}
/*
*/
static void geofence_send_status(mavlink_channel_t chan)
{
if (geofence_enabled() && geofence_state != NULL) {
mavlink_msg_fence_status_send(chan,
(int8_t)geofence_state->fence_triggered,
geofence_state->breach_count,
geofence_state->breach_type,
geofence_state->breach_time);
}
}
#else // GEOFENCE_ENABLED
static void geofence_check(bool altitude_check_only) { }
static bool geofence_stickmixing(void) { return true; }
static bool geofence_enabled(void) { return false; }
#endif // GEOFENCE_ENABLED

195
APMrover2/navigation.pde Normal file
View File

@ -0,0 +1,195 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
static void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (g_gps->fix == 0)
{
g_gps->new_data = false;
return;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
if((next_WP.lat == 0)||(home_is_set==false)){
#else
if(next_WP.lat == 0){
#endif
return;
}
if(control_mode < INITIALISING) {
// waypoint distance from plane
// ----------------------------
wp_distance = get_distance(&current_loc, &next_WP);
if (wp_distance < 0){
gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
//Serial.println(wp_distance,DEC);
return;
}
// 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;
// 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);
}
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
}
#if 0
// Disabled for now
void calc_distance_error()
{
distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
wp_distance = max(distance_estimate,10);
}
#endif
static void calc_gndspeed_undershoot()
{
// Function is overkill, but here in case we want to add filtering later
groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - ground_speed) : 0;
}
static void calc_bearing_error()
{
if(takeoff_complete == true || g.compass_enabled == true) {
/*
most of the time we use the yaw sensor for heading, even if
we don't have a compass. The yaw sensor is drift corrected
in the DCM library. We only use the gps ground course
directly if we haven't completed takeoff, as the yaw drift
correction won't have had a chance to kick in. Drift
correction using the GPS typically takes 10 seconds or so
for a 180 degree correction.
*/
bearing_error = nav_bearing - ahrs.yaw_sensor;
} else {
// TODO: we need to use the Yaw gyro for in between GPS reads,
// maybe as an offset from a saved gryo value.
bearing_error = nav_bearing - ground_course;
}
bearing_error = wrap_180(bearing_error);
}
static void calc_altitude_error()
{
}
static long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
static long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
static void calc_turn_radius() // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
{
wp_radius = g_gps->ground_speed * 150 / g.roll_limit.get();
//Serial.println(wp_radius, DEC);
}
static void update_loiter()
{
float power;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1);
nav_bearing += (int)(9000.0 * (2.0 + power));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
/*
if (wp_distance < g.loiter_radius){
nav_bearing += 9000;
}else{
nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
}
update_crosstrack();
*/
nav_bearing = wrap_360(nav_bearing);
}
static void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
}
}
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&prev_WP, &next_WP); // Used for track following
}
static long get_distance(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 sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
static 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 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}
void reached_waypoint()
{
}

56
APMrover2/planner.pde Normal file
View File

@ -0,0 +1,56 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t planner_gcs(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
static const struct Menu::command planner_menu_commands[] PROGMEM = {
{"gcs", planner_gcs},
};
// A Macro to create the Menu
MENU(planner_menu, "planner", planner_menu_commands);
static int8_t
planner_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
planner_menu.run();
return 0;
}
static int8_t
planner_gcs(uint8_t argc, const Menu::arg *argv)
{
gcs0.init(&Serial);
#if USB_MUX_PIN > 0
// we don't have gcs3 if we have the USB mux setup
gcs3.init(&Serial3);
#endif
int loopcount = 0;
while (1) {
if (millis()-fast_loopTimer > 19) {
fast_loopTimer = millis();
gcs_update();
read_radio();
gcs_data_stream_send(45,1000);
if ((loopcount % 5) == 0) // 10 hz
gcs_data_stream_send(5,45);
if ((loopcount % 16) == 0) { // 3 hz
gcs_data_stream_send(1,5);
gcs_send_message(MSG_HEARTBEAT);
}
loopcount++;
}
}
return 0;
}

263
APMrover2/radio.pde Normal file
View File

@ -0,0 +1,263 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
static void init_rc_in()
{
// set rc reversing
update_servo_switches();
// set rc channel ranges
g.channel_roll.set_angle(SERVO_MAX);
g.channel_pitch.set_angle(SERVO_MAX);
g.channel_rudder.set_angle(SERVO_MAX);
g.channel_throttle.set_range(0, 100);
// set rc dead zones
g.channel_roll.set_dead_zone(60);
g.channel_pitch.set_dead_zone(60);
g.channel_rudder.set_dead_zone(60);
g.channel_throttle.set_dead_zone(6);
//g.channel_roll.dead_zone = 60;
//g.channel_pitch.dead_zone = 60;
//g.channel_rudder.dead_zone = 60;
//g.channel_throttle.dead_zone = 6;
//set auxiliary ranges
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
}
static void init_rc_out()
{
APM_RC.Init( &isr_registry ); // APM Radio initialization
APM_RC.enable_out(CH_1);
APM_RC.enable_out(CH_2);
APM_RC.enable_out(CH_3);
APM_RC.enable_out(CH_4);
APM_RC.enable_out(CH_5);
APM_RC.enable_out(CH_6);
APM_RC.enable_out(CH_7);
APM_RC.enable_out(CH_8);
#if HIL_MODE != HIL_MODE_ATTITUDE
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
#else
APM_RC.OutputCh(CH_1, 1500); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, 1500);
APM_RC.OutputCh(CH_3, 1000);
APM_RC.OutputCh(CH_4, 1500);
APM_RC.OutputCh(CH_5, 1500);
APM_RC.OutputCh(CH_6, 1500);
APM_RC.OutputCh(CH_7, 1500);
APM_RC.OutputCh(CH_8, 2000);
#endif
}
static void read_radio()
{
static uint16_t aileron1;
static uint16_t aileron2;
ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH);
if(g.mix_mode == 0){
g.channel_roll.set_pwm(ch1_temp);
g.channel_pitch.set_pwm(ch2_temp);
}else{
g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
}
g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
#if FLAPERON == ENABLED
// JLN update for true flaperons
if (control_mode == MANUAL) {
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
} else {
aileron1 = g.rc_5.radio_trim + (BOOL_TO_SIGN(-g.rc_5.get_reverse()) *g.channel_roll.angle_to_pwm());
aileron2 = g.rc_6.radio_trim + (BOOL_TO_SIGN(-g.rc_6.get_reverse()) *g.channel_roll.angle_to_pwm());
aileron1 = constrain(aileron1,(uint16_t)g.rc_5.radio_min,(uint16_t)g.rc_5.radio_max);
aileron2 = constrain(aileron2,(uint16_t)g.rc_6.radio_min,(uint16_t)g.rc_6.radio_max);
g.rc_5.set_pwm(aileron1);
g.rc_6.set_pwm(aileron2);
}
#else
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
#endif
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
// TO DO - go through and patch throttle reverse for RC_Channel library compatibility
#if THROTTLE_REVERSE == 1
g.channel_throttle.radio_in = g.channel_throttle.radio_max + g.channel_throttle.radio_min - g.channel_throttle.radio_in;
#endif
control_failsafe(g.channel_throttle.radio_in);
g.channel_throttle.servo_out = g.channel_throttle.control_in;
if (g.channel_throttle.servo_out > 50) {
if(g.airspeed_enabled == true) {
airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
airspeed_nudge = g.nudgeoffset + airspeed_nudge;
} else {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
}
} else {
airspeed_nudge = g.nudgeoffset;
throttle_nudge = 0;
}
/*
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
g.rc_4.control_in);
*/
}
static void control_failsafe(uint16_t pwm)
{
if(g.throttle_fs_enabled == 0)
return;
// Check for failsafe condition based on loss of GCS control
if (rc_override_active) {
if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) {
ch3_failsafe = true;
} else {
ch3_failsafe = false;
}
//Check for failsafe and debounce funky reads
} else if (g.throttle_fs_enabled) {
if (pwm < (unsigned)g.throttle_fs_value){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9){
gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm);
}else if(failsafeCounter == 10) {
ch3_failsafe = true;
}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){
gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm);
}else if(failsafeCounter == 0) {
ch3_failsafe = false;
}else if (failsafeCounter <0){
failsafeCounter = -1;
}
}
}
}
static void trim_control_surfaces()
{
read_radio();
// Store control surface trim values
// ---------------------------------
if(g.mix_mode == 0){
if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) {
g.channel_roll.radio_trim = g.channel_roll.radio_max + g.channel_roll.radio_min - g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_max + g.channel_pitch.radio_min - g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_max + g.channel_rudder.radio_min - g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else {
g.channel_roll.radio_trim = 1500; // case of HIL test without receiver active
g.channel_pitch.radio_trim = 1500;
g.channel_rudder.radio_trim = 1500;
g.channel_throttle.radio_trim = 1000;
G_RC_AUX(k_aileron)->radio_trim = 1500;
}
}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;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
}
// save to eeprom
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
G_RC_AUX(k_aileron)->save_eeprom();
}
static void trim_radio()
{
for (int y = 0; y < 30; y++) {
read_radio();
}
// Store the trim values
// ---------------------
if(g.mix_mode == 0){
if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) {
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else {
g.channel_roll.radio_trim = 1500; // case of HIL test without receiver active
g.channel_pitch.radio_trim = 1500;
g.channel_rudder.radio_trim = 1500;
g.channel_throttle.radio_trim = 1000;
G_RC_AUX(k_aileron)->radio_trim = 1500;
}
} else {
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
}
// save to eeprom
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
G_RC_AUX(k_aileron)->save_eeprom();
}

106
APMrover2/sensors.pde Normal file
View File

@ -0,0 +1,106 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
void ReadSCP1000(void) {}
static void init_barometer(void)
{
int flashcount = 0;
long ground_pressure = 0;
int ground_temperature;
while (ground_pressure == 0 || !barometer.healthy) {
barometer.read(); // Get initial data from absolute pressure sensor
ground_pressure = baro_filter.apply(barometer.get_pressure());
//ground_pressure = barometer.get_pressure();
ground_temperature = barometer.get_temperature();
mavlink_delay(20);
//Serial.printf("barometer.Press %ld\n", barometer.get_pressure());
}
for(int i = 0; i < 30; i++){ // We take some readings...
#if HIL_MODE == HIL_MODE_SENSORS
gcs_update(); // look for inbound hil packets
#endif
do {
barometer.read(); // Get pressure sensor
} while (!barometer.healthy);
ground_pressure = baro_filter.apply(barometer.get_pressure());
//ground_pressure = (ground_pressure * 9l + barometer.get_pressure()) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.get_temperature()) / 10;
mavlink_delay(20);
if(flashcount == 5) {
digitalWrite(C_LED_PIN, LED_OFF);
digitalWrite(A_LED_PIN, LED_ON);
digitalWrite(B_LED_PIN, LED_OFF);
}
if(flashcount >= 10) {
flashcount = 0;
digitalWrite(C_LED_PIN, LED_ON);
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(B_LED_PIN, LED_ON);
}
flashcount++;
}
g.ground_pressure.set_and_save(ground_pressure);
g.ground_temperature.set_and_save(ground_temperature / 10.0f);
abs_pressure = ground_pressure;
Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete."));
}
static int32_t read_barometer(void)
{
float x, scaling, temp;
barometer.read(); // Get new data from absolute pressure sensor
float abs_pressure = baro_filter.apply(barometer.get_pressure());
//abs_pressure = (abs_pressure + barometer.get_pressure()) >> 1; // Small filtering
//abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.get_pressure() * .3); // large filtering
scaling = (float)g.ground_pressure / (float)abs_pressure;
temp = ((float)g.ground_temperature) + 273.15f;
x = log(scaling) * temp * 29271.267f;
return (x / 10);
}
// in M/S * 100
static void read_airspeed(void)
{
}
static void zero_airspeed(void)
{
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void read_battery(void)
{
if(g.battery_monitoring == 0) {
battery_voltage1 = 0;
return;
}
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
if(g.battery_monitoring == 4) {
current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin
current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
}
#if BATTERY_EVENT == ENABLED
if(battery_voltage1 < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity) low_battery_event();
#endif
}

604
APMrover2/setup.pde Normal file
View File

@ -0,0 +1,604 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
// 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);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
// Command/function table for the setup menu
static const struct Menu::command setup_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"reset", setup_factory},
{"radio", setup_radio},
{"modes", setup_flightmodes},
{"compass", setup_compass},
{"declination", setup_declination},
{"battery", setup_batt_monitor},
{"show", setup_show},
{"erase", setup_erase},
};
// Create the setup menu object.
MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu.
static 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();
return 0;
}
// Print the current configuration.
// Called by the setup menu 'show' command.
static int8_t
setup_show(uint8_t argc, const Menu::arg *argv)
{
// clear the area
print_blanks(8);
report_radio();
report_batt_monitor();
report_gains();
report_xtrack();
report_throttle();
report_flight_modes();
report_imu();
report_compass();
Serial.printf_P(PSTR("Raw Values\n"));
print_divider();
AP_Param::show_all();
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)
{
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);
AP_Param::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
for (;;) {
}
// note, cannot actually return here
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.printf_P(PSTR("\n\nRadio Setup:\n"));
uint8_t i;
for(i = 0; i < 100;i++){
delay(20);
read_radio();
}
if(g.channel_roll.radio_in < 500){
while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000);
// stop here
}
}
g.channel_roll.radio_min = g.channel_roll.radio_in;
g.channel_pitch.radio_min = g.channel_pitch.radio_in;
g.channel_throttle.radio_min = g.channel_throttle.radio_in;
g.channel_rudder.radio_min = g.channel_rudder.radio_in;
g.rc_5.radio_min = g.rc_5.radio_in;
g.rc_6.radio_min = g.rc_6.radio_in;
g.rc_7.radio_min = g.rc_7.radio_in;
g.rc_8.radio_min = g.rc_8.radio_in;
g.channel_roll.radio_max = g.channel_roll.radio_in;
g.channel_pitch.radio_max = g.channel_pitch.radio_in;
g.channel_throttle.radio_max = g.channel_throttle.radio_in;
g.channel_rudder.radio_max = g.channel_rudder.radio_in;
g.rc_5.radio_max = g.rc_5.radio_in;
g.rc_6.radio_max = g.rc_6.radio_in;
g.rc_7.radio_max = g.rc_7.radio_in;
g.rc_8.radio_max = g.rc_8.radio_in;
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
g.rc_5.radio_trim = 1500;
g.rc_6.radio_trim = 1500;
g.rc_7.radio_trim = 1500;
g.rc_8.radio_trim = 1500;
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
g.channel_roll.update_min_max();
g.channel_pitch.update_min_max();
g.channel_throttle.update_min_max();
g.channel_rudder.update_min_max();
g.rc_5.update_min_max();
g.rc_6.update_min_max();
g.rc_7.update_min_max();
g.rc_8.update_min_max();
if(Serial.available() > 0){
Serial.flush();
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
g.rc_5.save_eeprom();
g.rc_6.save_eeprom();
g.rc_7.save_eeprom();
g.rc_8.save_eeprom();
print_done();
break;
}
}
trim_radio();
report_radio();
return(0);
}
static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
byte switchPosition, mode = 0;
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){
// force position 5 to MANUAL
if (switchPosition > 4) {
flight_modes[switchPosition] = MANUAL;
}
// update our current mode
mode = flight_modes[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)
{
if (mode < MANUAL)
mode = LOITER;
else if (mode >LOITER)
mode = MANUAL;
else
mode += radioInputSwitch;
}
// Override position 5
if(switchPosition > 4)
mode = MANUAL;
// save new mode
flight_modes[switchPosition] = mode;
// print new mode
print_switch(switchPosition, mode);
}
// escape hatch
if(Serial.available() > 0){
// save changes
for (mode=0; mode<6; mode++)
flight_modes[mode].save();
report_flight_modes();
print_done();
return (0);
}
}
}
static int8_t
setup_declination(uint8_t argc, const Menu::arg *argv)
{
compass.set_declination(radians(argv[1].f));
report_compass();
return 0;
}
static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv)
{
int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
zero_eeprom();
return 0;
}
static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
g.compass_enabled = true;
}
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.compass_enabled = false;
} else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
compass.set_offsets(0,0,0);
} else {
Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n"));
report_compass();
return 0;
}
g.compass_enabled.save();
report_compass();
return 0;
}
static int8_t
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
{
if(argv[1].i >= 0 && argv[1].i <= 4){
g.battery_monitoring.set_and_save(argv[1].i);
} else {
Serial.printf_P(PSTR("\nOptions: 3-4"));
}
report_batt_monitor();
return 0;
}
/***************************************************************************/
// CLI reports
/***************************************************************************/
static void report_batt_monitor()
{
//print_blanks(2);
Serial.printf_P(PSTR("Batt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
print_blanks(2);
}
static void report_radio()
{
//print_blanks(2);
Serial.printf_P(PSTR("Radio\n"));
print_divider();
// radio
print_radio_values();
print_blanks(2);
}
static void report_gains()
{
//print_blanks(2);
Serial.printf_P(PSTR("Gains\n"));
print_divider();
Serial.printf_P(PSTR("servo roll:\n"));
print_PID(&g.pidServoRoll);
Serial.printf_P(PSTR("servo pitch:\n"));
print_PID(&g.pidServoPitch);
Serial.printf_P(PSTR("servo rudder:\n"));
print_PID(&g.pidServoRudder);
Serial.printf_P(PSTR("nav roll:\n"));
print_PID(&g.pidNavRoll);
Serial.printf_P(PSTR("nav pitch airspeed:\n"));
print_PID(&g.pidNavPitchAirspeed);
Serial.printf_P(PSTR("energry throttle:\n"));
print_PID(&g.pidTeThrottle);
Serial.printf_P(PSTR("nav pitch alt:\n"));
print_PID(&g.pidNavPitchAltitude);
print_blanks(2);
}
static void report_xtrack()
{
//print_blanks(2);
Serial.printf_P(PSTR("Crosstrack\n"));
print_divider();
// radio
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"),
(float)g.crosstrack_gain,
(int)g.crosstrack_entry_angle);
print_blanks(2);
}
static void report_throttle()
{
//print_blanks(2);
Serial.printf_P(PSTR("Throttle\n"));
print_divider();
Serial.printf_P(PSTR("min: %d\n"
"max: %d\n"
"cruise: %d\n"
"failsafe_enabled: %d\n"
"failsafe_value: %d\n"),
(int)g.throttle_min,
(int)g.throttle_max,
(int)g.throttle_cruise,
(int)g.throttle_fs_enabled,
(int)g.throttle_fs_value);
print_blanks(2);
}
static void report_imu()
{
//print_blanks(2);
Serial.printf_P(PSTR("IMU\n"));
print_divider();
print_gyro_offsets();
print_accel_offsets();
print_blanks(2);
}
static void report_compass()
{
//print_blanks(2);
Serial.printf_P(PSTR("Compass: "));
switch (compass.product_id) {
case AP_COMPASS_TYPE_HMC5883L:
Serial.println_P(PSTR("HMC5883L"));
break;
case AP_COMPASS_TYPE_HMC5843:
Serial.println_P(PSTR("HMC5843"));
break;
case AP_COMPASS_TYPE_HIL:
Serial.println_P(PSTR("HIL"));
break;
default:
Serial.println_P(PSTR("??"));
break;
}
print_divider();
print_enabled(g.compass_enabled);
// mag declination
Serial.printf_P(PSTR("Mag Declination: %4.4f\n"),
degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets();
// mag offsets
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
offsets.x,
offsets.y,
offsets.z);
print_blanks(2);
}
static void report_flight_modes()
{
//print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n"));
print_divider();
for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]);
}
print_blanks(2);
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
static void
print_PID(PID * pid)
{
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
pid->kP(),
pid->kI(),
pid->kD(),
(long)pid->imax());
}
static void
print_radio_values()
{
Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
}
static void
print_switch(byte p, byte m)
{
Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]);
}
static void
print_done()
{
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
}
static void
print_blanks(int num)
{
while(num > 0){
num--;
Serial.println("");
}
}
static void
print_divider(void)
{
for (int i = 0; i < 40; i++) {
Serial.printf_P(PSTR("-"));
}
Serial.println("");
}
static int8_t
radio_input_switch(void)
{
static int8_t bouncer = 0;
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) {
bouncer = 10;
}
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) {
bouncer = -10;
}
if (bouncer >0) {
bouncer --;
}
if (bouncer <0) {
bouncer ++;
}
if (bouncer == 1 || bouncer == -1) {
return bouncer;
} else {
return 0;
}
}
static void zero_eeprom(void)
{
byte b = 0;
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b);
}
Serial.printf_P(PSTR("done\n"));
}
static void print_enabled(bool b)
{
if(b)
Serial.printf_P(PSTR("en"));
else
Serial.printf_P(PSTR("dis"));
Serial.printf_P(PSTR("abled\n"));
}
static void
print_accel_offsets(void)
{
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
(float)imu.ax(),
(float)imu.ay(),
(float)imu.az());
}
static void
print_gyro_offsets(void)
{
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
(float)imu.gx(),
(float)imu.gy(),
(float)imu.gz());
}
#endif // CLI_ENABLED

583
APMrover2/system.pde Normal file
View File

@ -0,0 +1,583 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*****************************************************************************
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.
*****************************************************************************/
#if CLI_ENABLED == ENABLED
// Functions called from the top-level menu
static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
// 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.
static const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"logs", process_logs},
{"setup", setup_mode},
{"test", test_mode},
{"help", main_menu_help},
{"planner", planner_mode}
};
// Create the top-level menu object.
MENU(main_menu, THISFIRMWARE, main_menu_commands);
// the user wants the CLI. It never exits
static void run_cli(void)
{
// disable the failsafe code in the CLI
timer_scheduler.set_failsafe(NULL);
while (1) {
main_menu.run();
}
}
#endif // CLI_ENABLED
static void init_ardupilot()
{
#if USB_MUX_PIN > 0
// on the APM2 board we have a mux thet switches UART0 between
// USB and the board header. If the right ArduPPM firmware is
// installed we can detect if USB is connected using the
// USB_MUX_PIN
pinMode(USB_MUX_PIN, INPUT);
usb_connected = !digitalRead(USB_MUX_PIN);
if (!usb_connected) {
// USB is not connected, this means UART0 may be a Xbee, with
// its darned bricking problem. We can't write to it for at
// least one second after powering up. Simplest solution for
// now is to delay for 1 second. Something more elegant may be
// added later
delay(1000);
}
#endif
// 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.
//
// 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.
//
// standard gps running
Serial1.begin(115200, 128, 16);
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %u\n"),
memcheck_available_memory());
#if QUATERNION_ENABLE == ENABLED
Serial.printf_P(PSTR("Quaternion test\n"));
#endif
//
// Initialize Wire and SPI libraries
//
#ifndef DESKTOP_BUILD
I2c.begin();
I2c.timeOut(5);
// initially set a fast I2c speed, and drop it on first failures
I2c.setSpeed(true);
#endif
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
//
// Initialize the ISR registry.
//
isr_registry.init();
//
// Initialize the timer scheduler to use the ISR registry.
//
timer_scheduler.init( & isr_registry );
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
load_parameters();
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// init the GCS
gcs0.init(&Serial);
#if USB_MUX_PIN > 0
if (!usb_connected) {
// we are not connected via USB, re-init UART0 with right
// baud rate
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
}
#else
// we have a 2nd serial port for telemetry
Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
gcs3.init(&Serial3);
#endif
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
DataFlash.Init(); // DataFlash log initialization
if (!DataFlash.CardInserted()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
g.log_bitmask.set(0);
} else if (DataFlash.NeedErase()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
do_erase_logs();
}
if (g.log_bitmask != 0) {
DataFlash.start_new_log();
}
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
#if CONFIG_ADC == ENABLED
adc.Init(&timer_scheduler); // APM ADC library initialization
#endif
barometer.init(&timer_scheduler);
if (g.compass_enabled==true) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()|| !compass.read()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
//compass.get_offsets(); // load offsets to account for airframe magnetic interference
compass.null_offsets_enable();
}
}
#endif
// Do GPS init
g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization
g_gps->callback = mavlink_delay;
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
mavlink_system.type = MAV_FIXED_WING;
rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override
RC_Channel::set_apm_rc( &APM_RC ); // Provide reference to RC outputs.
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
#if SLIDE_SWITCH_PIN > 0
pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode
#endif
#if CONFIG_PUSHBUTTON == ENABLED
pinMode(PUSHBUTTON_PIN, INPUT); // unused
#endif
#if CONFIG_RELAY == ENABLED
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
#endif
#if FENCE_TRIGGERED_PIN > 0
pinMode(FENCE_TRIGGERED_PIN, OUTPUT);
digitalWrite(FENCE_TRIGGERED_PIN, LOW);
#endif
/*
setup the 'main loop is dead' check. Note that this relies on
the RC library being initialised.
*/
timer_scheduler.set_failsafe(failsafe_check);
// 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 CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED
if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
digitalWrite(A_LED_PIN,LED_ON); // 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"));
Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
run_cli();
}
#else
Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n"));
#endif // CLI_ENABLED
// read in the flight switches
update_servo_switches();
startup_ground();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
set_mode(MANUAL);
// set the correct flight mode
// ---------------------------
reset_control_switch();
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
static void startup_ground(void)
{
set_mode(INITIALISING);
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
#if(GROUND_START_DELAY > 0)
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> With Delay"));
delay(GROUND_START_DELAY * 1000);
#endif
// Makes the servos wiggle
// step 1 = 1 wiggle
// -----------------------
demo_servos(1);
//IMU ground start
//------------------------
//
startup_IMU_ground(false);
// read the radio to set trims
// ---------------------------
trim_radio(); // This was commented out as a HACK. Why? I don't find a problem.
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
// initialize commands
// -------------------
init_commands();
// Read in the GPS - see if one is connected
GPS_enabled = false;
for (byte counter = 0; ; counter++) {
g_gps->update();
if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){
GPS_enabled = true;
break;
}
if (counter >= 2) {
GPS_enabled = false;
break;
}
}
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
demo_servos(3);
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}
static void set_mode(byte mode)
{ struct Location temp;
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
control_mode = mode;
crash_timer = 0;
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case CIRCLE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
break;
case AUTO:
reload_commands();
update_auto();
break;
case RTL:
do_RTL();
break;
case LOITER:
do_loiter_at_location();
break;
case GUIDED:
set_guided_WP();
break;
default:
//do_RTL();
break;
}
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
static void check_long_failsafe()
{
// only act on changes
// -------------------
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){
if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
failsafe_long_on_event(FAILSAFE_LONG);
}
if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) {
failsafe_long_on_event(FAILSAFE_LONG);
}
if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
failsafe_long_on_event(FAILSAFE_GCS);
}
} else {
// We do not change state but allow for user to change mode
if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE;
}
}
static void check_short_failsafe()
{
// only act on changes
// -------------------
if(failsafe == FAILSAFE_NONE){
if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
failsafe_short_on_event(FAILSAFE_SHORT);
}
}
if(failsafe == FAILSAFE_SHORT){
if(!ch3_failsafe) {
failsafe_short_off_event();
}
}
}
static void startup_IMU_ground(bool force_accel_level)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
// -----------------------
demo_servos(2);
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane"));
mavlink_delay(1000);
imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
if (force_accel_level || g.manual_level == 0) {
// when MANUAL_LEVEL is set to 1 we don't do accelerometer
// levelling on each boot, and instead rely on the user to do
// it once via the ground station
imu.init_accel(mavlink_delay, flash_leds);
}
ahrs.set_centripetal(1);
ahrs.reset();
// read Baro pressure at ground
//-----------------------------
init_barometer();
if (g.airspeed_enabled == true) {
// initialize airspeed sensor
// --------------------------
zero_airspeed();
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> zero airspeed calibrated"));
} else {
gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> NO airspeed"));
}
#endif // HIL_MODE_ATTITUDE
digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate IMU ready
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF);
}
static void update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
switch (g_gps->status()) {
case(2):
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix.
break;
case(1):
if (g_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, LED_OFF);
} else {
digitalWrite(C_LED_PIN, LED_ON);
}
g_gps->valid_read = false;
}
break;
default:
digitalWrite(C_LED_PIN, LED_OFF);
break;
}
}
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
imu.adc_constraints = 0;
ahrs.renorm_range_count = 0;
ahrs.renorm_blowup_count = 0;
gps_fix_count = 0;
pmTest1 = 0;
perf_mon_timer = millis();
}
/*
map from a 8 bit EEPROM baud rate to a real baud rate
*/
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
{
switch (rate) {
case 1: return 1200;
case 2: return 2400;
case 4: return 4800;
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 111: return 111100;
case 115: return 115200;
}
Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
return default_baud;
}
#if USB_MUX_PIN > 0
static void check_usb_mux(void)
{
bool usb_check = !digitalRead(USB_MUX_PIN);
if (usb_check == usb_connected) {
return;
}
// the user has switched to/from the telemetry port
usb_connected = usb_check;
if (usb_connected) {
Serial.begin(SERIAL0_BAUD, 128, 128);
} else {
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
}
}
#endif
/*
called by gyro/accel init to flash LEDs so user
has some mesmerising lights to watch while waiting
*/
void flash_leds(bool on)
{
digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON);
digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
}
#ifndef DESKTOP_BUILD
/*
* Read Vcc vs 1.1v internal reference
*
* This call takes about 150us total. ADC conversion is 13 cycles of
* 125khz default changes the mux if it isn't set, and return last
* reading (allows necessary settle time) otherwise trigger the
* conversion
*/
uint16_t board_voltage(void)
{
const uint8_t mux = (_BV(REFS0)|_BV(MUX4)|_BV(MUX3)|_BV(MUX2)|_BV(MUX1));
if (ADMUX == mux) {
ADCSRA |= _BV(ADSC); // Convert
uint16_t counter=4000; // normally takes about 1700 loops
while (bit_is_set(ADCSRA, ADSC) && counter) // Wait
counter--;
if (counter == 0) {
// we don't actually expect this timeout to happen,
// but we don't want any more code that could hang. We
// report 0V so it is clear in the logs that we don't know
// the value
return 0;
}
uint32_t result = ADCL | ADCH<<8;
return 1126400UL / result; // Read and back-calculate Vcc in mV
}
// switch mux, settle time is needed. We don't want to delay
// waiting for the settle, so report 0 as a "don't know" value
ADMUX = mux;
return 0; // we don't know the current voltage
}
#endif

695
APMrover2/test.pde Normal file
View File

@ -0,0 +1,695 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
// 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_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
#if CONFIG_ADC == ENABLED
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
#endif
static int8_t test_imu(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_vario(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);
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
#endif
static int8_t test_logging(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
static const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"passthru", test_passthru},
{"failsafe", test_failsafe},
{"battery", test_battery},
{"relay", test_relay},
{"waypoints", test_wp},
{"xbee", test_xbee},
{"eedump", test_eedump},
{"modeswitch", test_modeswitch},
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
{"dipswitches", test_dipswitches},
#endif
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
#if HIL_MODE == HIL_MODE_DISABLED
#if CONFIG_ADC == ENABLED
{"adc", test_adc},
#endif
{"gps", test_gps},
{"rawgps", test_rawgps},
{"imu", test_imu},
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
{"vario", test_vario},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_SENSORS
{"adc", test_adc},
{"gps", test_gps},
{"imu", test_imu},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_ATTITUDE
#endif
{"logging", test_logging},
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
static int8_t
test_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run();
return 0;
}
static 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_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_passthru(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
// New radio frame? (we could use also if((millis()- timer) > 20)
if (APM_RC.GetState() == 1){
Serial.print("CH:");
for(int i = 0; i < 8; i++){
Serial.print(APM_RC.InputCh(i)); // Print channel values
Serial.print(",");
APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
}
Serial.println();
}
if (Serial.available() > 0){
return (0);
}
}
return 0;
}
static int8_t
test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
#if THROTTLE_REVERSE == 1
Serial.printf_P(PSTR("Throttle is reversed in config: \n"));
delay(1000);
#endif
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
update_servo_switches();
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_throttle.calc_pwm();
g.channel_rudder.calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
tuning_value = constrain(((float)(g.rc_7.radio_in - g.rc_7.radio_min) / (float)(g.rc_7.radio_max - g.rc_7.radio_min)),0,1);
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d Tuning = %2.3f\n"),
g.channel_roll.control_in,
g.channel_pitch.control_in,
g.channel_throttle.control_in,
g.channel_rudder.control_in,
g.rc_5.control_in,
g.rc_6.control_in,
g.rc_7.control_in,
g.rc_8.control_in,
tuning_value);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv)
{
byte fail_test;
print_hit_enter();
for(int i = 0; i < 50; i++){
delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(g.channel_throttle.control_in > 0){
delay(20);
read_radio();
}
while(1){
delay(20);
read_radio();
if(g.channel_throttle.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in);
fail_test++;
}
if(oldSwitchPosition != readSwitch()){
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
if(fail_test > 0){
return (0);
}
if(Serial.available() > 0){
Serial.printf_P(PSTR("LOS caused no change in APM.\n"));
return (0);
}
}
}
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
print_hit_enter();
delta_ms_medium_loop = 100;
while(1){
delay(100);
read_radio();
read_battery();
if (g.battery_monitoring == 3){
Serial.printf_P(PSTR("V: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
} else {
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
}
// write out the servo PWM values
// ------------------------------
set_servos();
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Not enabled\n"));
return (0);
}
}
static int8_t
test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
Serial.printf_P(PSTR("Relay on\n"));
relay.on();
delay(3000);
if(Serial.available() > 0){
return (0);
}
Serial.printf_P(PSTR("Relay off\n"));
relay.off();
delay(3000);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
// save the alitude above home option
if(g.RTL_altitude < 0){
Serial.printf_P(PSTR("Hold current altitude\n"));
}else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100);
}
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(byte i = 0; i <= g.command_total; i++){
struct Location temp = get_cmd_with_index(i);
test_wp_print(&temp, i);
}
return (0);
}
static void
test_wp_print(struct Location *cmd, byte wp_index)
{
Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
cmd->alt,
cmd->lat,
cmd->lng);
}
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){
if (Serial3.available())
Serial3.write(Serial3.read());
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("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);
}
}
}
/*
test the dataflash is working
*/
static int8_t
test_logging(uint8_t argc, const Menu::arg *argv)
{
Serial.println_P(PSTR("Testing dataflash logging"));
if (!DataFlash.CardInserted()) {
Serial.println_P(PSTR("ERR: No dataflash inserted"));
return 0;
}
DataFlash.ReadManufacturerID();
Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"),
(unsigned)DataFlash.df_manufacturer,
(unsigned)DataFlash.df_device);
Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"),
(unsigned)DataFlash.df_NumPages+1,
(unsigned)DataFlash.df_PageSize);
DataFlash.StartRead(DataFlash.df_NumPages+1);
Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"),
(unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT);
return 0;
}
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
static int8_t
test_dipswitches(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
if (!g.switch_enable) {
Serial.println_P(PSTR("dip switches disabled, using EEPROM"));
}
while(1){
delay(100);
update_servo_switches();
if (g.mix_mode == 0) {
Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
(int)g.channel_roll.get_reverse(),
(int)g.channel_pitch.get_reverse(),
(int)g.channel_rudder.get_reverse());
} else {
Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
(int)g.reverse_elevons,
(int)g.reverse_ch1_elevon,
(int)g.reverse_ch2_elevon);
}
if(Serial.available() > 0){
return (0);
}
}
}
#endif // CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
//-------------------------------------------------------------------------------------------
// 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
#if CONFIG_ADC == ENABLED
static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init(&timer_scheduler);
delay(1000);
Serial.printf_P(PSTR("ADC\n"));
delay(1000);
while(1){
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%.1f\t"),adc.Ch(i));
Serial.println();
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
#endif // CONFIG_ADC
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();
g_gps->update();
if (g_gps->new_data){
Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
g_gps->latitude,
g_gps->longitude,
g_gps->altitude/100,
g_gps->num_sats);
}else{
Serial.printf_P(PSTR("."));
}
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(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
ahrs.reset();
print_hit_enter();
delay(1000);
while(1){
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
ahrs.update();
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5){
if (compass.read()) {
compass.calculate(ahrs.get_dcm_matrix()); // Calculate heading
}
medium_loopCounter = 0;
}
}
// We are using the IMU
// ---------------------
Vector3f gyros = imu.get_gyro();
Vector3f accels = imu.get_accel();
Serial.printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"),
(int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100,
(uint16_t)ahrs.yaw_sensor / 100,
gyros.x, gyros.y, gyros.z,
accels.x, accels.y, accels.z);
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_mag(uint8_t argc, const Menu::arg *argv)
{
if (!g.compass_enabled) {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
compass.set_orientation(MAG_ORIENTATION);
if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
return 0;
}
compass.null_offsets_enable();
ahrs.set_compass(&compass);
report_compass();
// we need the AHRS initialised for this test
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
ahrs.reset();
int counter = 0;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter();
while(1) {
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
ahrs.update();
medium_loopCounter++;
if(medium_loopCounter == 5){
if (compass.read()) {
// Calculate heading
Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m);
compass.null_offsets();
}
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
if (compass.healthy) {
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
(int)compass.mag_x,
(int)compass.mag_y,
(int)compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
} else {
Serial.println_P(PSTR("compass not healthy"));
}
counter=0;
}
}
if (Serial.available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
Serial.println_P(PSTR("saving offsets"));
compass.save_offsets();
return (0);
}
#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)
{
}
static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv)
{
}
static int8_t
test_vario(uint8_t argc, const Menu::arg *argv)
{
}
static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
if (Serial3.available()){
digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS
Serial1.write(Serial3.read());
digitalWrite(B_LED_PIN, LED_OFF);
}
if (Serial1.available()){
digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS
Serial3.write(Serial1.read());
digitalWrite(C_LED_PIN, LED_OFF);
}
if(Serial.available() > 0){
return (0);
}
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED