diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h new file mode 100644 index 0000000000..3a73abca02 --- /dev/null +++ b/APMrover2/APM_Config.h @@ -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 +*/ + + diff --git a/APMrover2/APM_Config.h.org b/APMrover2/APM_Config.h.org new file mode 100644 index 0000000000..0b40f0e887 --- /dev/null +++ b/APMrover2/APM_Config.h.org @@ -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 +*/ diff --git a/APMrover2/APM_Config.h.reference b/APMrover2/APM_Config.h.reference new file mode 100644 index 0000000000..fed8532b2e --- /dev/null +++ b/APMrover2/APM_Config.h.reference @@ -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. diff --git a/APMrover2/APM_Config_HILmode.h b/APMrover2/APM_Config_HILmode.h new file mode 100644 index 0000000000..90d924c5d5 --- /dev/null +++ b/APMrover2/APM_Config_HILmode.h @@ -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 +// diff --git a/APMrover2/APM_Config_Rover.h b/APMrover2/APM_Config_Rover.h new file mode 100644 index 0000000000..db28b8eff8 --- /dev/null +++ b/APMrover2/APM_Config_Rover.h @@ -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 +// diff --git a/APMrover2/APM_Config_mavlink_hil.h b/APMrover2/APM_Config_mavlink_hil.h new file mode 100644 index 0000000000..38cff467b5 --- /dev/null +++ b/APMrover2/APM_Config_mavlink_hil.h @@ -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 diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde new file mode 100644 index 0000000000..765bcc2b43 --- /dev/null +++ b/APMrover2/APMrover2.pde @@ -0,0 +1,1135 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define THISFIRMWARE "APMrover v2.0a JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + +// This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN) + +/* +Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler +Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier +Please contribute your ideas! + + +This firmware is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +// +// JLN updates: last update 2012-04-30 +// +// DOLIST: +// +// +//------------------------------------------------------------------------------------------------------------------------- +// Dev Startup : 2012-04-21 +// +// 2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer +// 2012-04-27: Cosmetic changes +// 2012-04-26: Only one PID (pidNavRoll) for steering the wheel with nav_roll +// 2012-04-26: Added ground_speed and ground_course variables in Update_GPS +// 2012-04-26: Set GPS to 10 Hz (updated in the AP_GPS lib) +// 2012-04-22: Tested on Traxxas Monster Jam Grinder XL-5 3602 +// 2012-04-21: Roll set to wheels control and Throttle neutral to 50% (0 -100) - Forward>50, Backward<50 +// +// Radio setup: +// APM INPUT (Rec = receiver) +// Rec ch1: Roll +// Rec ch2: Throttle +// Rec ch3: Pitch +// Rec ch4: Yaw +// Rec ch5: not used +// Rec ch6: not used +// 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 +// +// more infos this experimental version: http://diydrones.com/profile/JeanLouisNaudin +// ======================================================================================================= +*/ + +//////////////////////////////////////////////////////////////////////////////// +// Header includes +//////////////////////////////////////////////////////////////////////////////// + +// AVR runtime +#include +#include +#include +#include + +// Libraries +#include +#include +#include +#include // ArduPilot Mega RC Library +#include // ArduPilot GPS library +#include // Wayne Truchsess I2C lib +#include // Arduino SPI lib +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega polymorphic analog getter +#include // ArduPilot Mega TimerProcess +#include // ArduPilot barometer library +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // Inertial Sensor (uncalibated IMU) Library +#include // ArduPilot Mega IMU Library +#include // ArduPilot Mega DCM Library +#include // PID library +#include // RC Channel Library +#include // Range finder library +#include // Filter library +#include // Mode Filter from Filter library +#include // Mode Filter from Filter library +#include // APM relay +#include // Camera/Antenna mount +#include // MAVLink GCS definitions +#include + +// Configuration +#include "config.h" + +// Local modules +#include "defines.h" +#include "Parameters.h" +#include "GCS.h" + +#include // ArduPilot Mega Declination Helper Library + +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +// +// Note that FastSerial port buffers are allocated at ::begin time, +// so there is not much of a penalty to defining ports that we don't +// use. +// +FastSerialPort0(Serial); // FTDI/console +FastSerialPort1(Serial1); // GPS port +FastSerialPort3(Serial3); // Telemetry port + +//////////////////////////////////////////////////////////////////////////////// +// ISR Registry +//////////////////////////////////////////////////////////////////////////////// +Arduino_Mega_ISR_Registry isr_registry; + + +//////////////////////////////////////////////////////////////////////////////// +// APM_RC_Class Instance +//////////////////////////////////////////////////////////////////////////////// +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + APM_RC_APM2 APM_RC; +#else + APM_RC_APM1 APM_RC; +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Dataflash +//////////////////////////////////////////////////////////////////////////////// +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + DataFlash_APM2 DataFlash; +#else + DataFlash_APM1 DataFlash; +#endif + + +//////////////////////////////////////////////////////////////////////////////// +// Parameters +//////////////////////////////////////////////////////////////////////////////// +// +// Global parameters are all contained within the 'g' class. +// +static Parameters g; + + +//////////////////////////////////////////////////////////////////////////////// +// prototypes +static void update_events(void); + + +//////////////////////////////////////////////////////////////////////////////// +// Sensors +//////////////////////////////////////////////////////////////////////////////// +// +// There are three basic options related to flight sensor selection. +// +// - Normal flight mode. Real sensors are used. +// - HIL Attitude mode. Most sensors are disabled, as the HIL +// protocol supplies attitude information directly. +// - HIL Sensors mode. Synthetic sensors are configured that +// supply data from the simulation. +// + +// All GPS access should be through this pointer. +static GPS *g_gps; + +// flight modes convenience array +static AP_Int8 *flight_modes = &g.flight_mode1; + +#if HIL_MODE == HIL_MODE_DISABLED + +// real sensors +#if CONFIG_ADC == ENABLED +static AP_ADC_ADS7844 adc; +#endif + +#ifdef DESKTOP_BUILD +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; +#else + +#if CONFIG_BARO == AP_BARO_BMP085 +# if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +static AP_Baro_BMP085 barometer(true); +# else +static AP_Baro_BMP085 barometer(false); +# endif +#elif CONFIG_BARO == AP_BARO_MS5611 +static AP_Baro_MS5611 barometer; +#endif + +static AP_Compass_HMC5843 compass; +#endif + +// real GPS selection +#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO +AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA +AP_GPS_NMEA g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF +AP_GPS_SIRF g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX +AP_GPS_UBLOX g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK +AP_GPS_MTK g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 +AP_GPS_MTK16 g_gps_driver(&Serial1); + +#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE +AP_GPS_None g_gps_driver(NULL); + +#else + #error Unrecognised GPS_PROTOCOL setting. +#endif // GPS PROTOCOL + +# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 + AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); +# else + AP_InertialSensor_Oilpan ins( &adc ); +#endif // CONFIG_IMU_TYPE +AP_IMU_INS imu( &ins ); + +#if QUATERNION_ENABLE == ENABLED + AP_AHRS_Quaternion ahrs(&imu, g_gps); +#else + AP_AHRS_DCM ahrs(&imu, g_gps); +#endif + +#elif HIL_MODE == HIL_MODE_SENSORS +// sensor emulators +AP_ADC_HIL adc; +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; +AP_GPS_HIL g_gps_driver(NULL); +AP_InertialSensor_Oilpan ins( &adc ); +AP_IMU_Shim imu; +AP_AHRS_DCM ahrs(&imu, g_gps); + +#elif HIL_MODE == HIL_MODE_ATTITUDE +AP_ADC_HIL adc; +AP_IMU_Shim imu; // never used +AP_AHRS_HIL ahrs(&imu, g_gps); +AP_GPS_HIL g_gps_driver(NULL); +AP_Compass_HIL compass; // never used +AP_Baro_BMP085_HIL barometer; +#else + #error Unrecognised HIL_MODE setting. +#endif // HIL MODE + +// we always have a timer scheduler +AP_TimerProcess timer_scheduler; + +//////////////////////////////////////////////////////////////////////////////// +// GCS selection +//////////////////////////////////////////////////////////////////////////////// +// +GCS_MAVLINK gcs0; +GCS_MAVLINK gcs3; + +//////////////////////////////////////////////////////////////////////////////// +// PITOT selection +//////////////////////////////////////////////////////////////////////////////// +// +ModeFilterInt16_Size5 sonar_mode_filter(2); + +#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC +AP_AnalogSource_ADC pitot_analog_source( &adc, + CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0); +#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN +AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0); +#endif + +#if SONAR_TYPE == MAX_SONAR_XL + AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); +#elif SONAR_TYPE == MAX_SONAR_LV + // XXX honestly I think these output the same values + // If someone knows, can they confirm it? + AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); +#endif + +// Barometer filter +AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration + +AP_Relay relay; + +// Camera/Antenna mount tracking and stabilisation stuff +// -------------------------------------- +#if MOUNT == ENABLED +AP_Mount camera_mount(g_gps, &dcm); +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Global variables +//////////////////////////////////////////////////////////////////////////////// + +// APM2 only +#if USB_MUX_PIN > 0 +static bool usb_connected; +#endif + +static const char *comma = ","; + +static const char* flight_mode_strings[] = { + "Manual", + "Circle", + "Stabilize", + "", + "", + "FBW_A", + "FBW_B", + "", + "", + "", + "Auto", + "RTL", + "Loiter", + "", + "", + "", + "", + "", + "", + "", + "", + ""}; + +/* Radio values + Channel assignments + 1 Ailerons (rudder if no ailerons) + 2 Elevator + 3 Throttle + 4 Rudder (if we have ailerons) + 5 Aux5 + 6 Aux6 + 7 Aux7 + 8 Aux8/Mode + Each Aux channel can be configured to have any of the available auxiliary functions assigned to it. + See libraries/RC_Channel/RC_Channel_aux.h for more information +*/ + +//////////////////////////////////////////////////////////////////////////////// +// Radio +//////////////////////////////////////////////////////////////////////////////// +// This is the state of the flight control system +// There are multiple states defined such as MANUAL, FBW-A, AUTO +byte control_mode = INITIALISING; +// Used to maintain the state of the previous control switch position +// This is set to -1 when we need to re-read the switch +byte oldSwitchPosition; +// This is used to enable the inverted flight feature +bool inverted_flight = false; +// These are trim values used for elevon control +// For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon +static uint16_t elevon1_trim = 1500; +static uint16_t elevon2_trim = 1500; +// These are used in the calculation of elevon1_trim and elevon2_trim +static uint16_t ch1_temp = 1500; +static uint16_t ch2_temp = 1500; +// These are values received from the GCS if the user is using GCS joystick +// control and are substituted for the values coming from the RC radio +static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; +// A flag if GCS joystick control is in use +static bool rc_override_active = false; + +//////////////////////////////////////////////////////////////////////////////// +// Failsafe +//////////////////////////////////////////////////////////////////////////////// +// A tracking variable for type of failsafe active +// Used for failsafe based on loss of RC signal or GCS signal +static int failsafe; +// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold +// RC receiver should be set up to output a low throttle value when signal is lost +static bool ch3_failsafe; +// A timer used to help recovery from unusual attitudes. If we enter an unusual attitude +// while in autonomous flight this variable is used to hold roll at 0 for a recovery period +static byte crash_timer; +// A timer used to track how long since we have received the last GCS heartbeat or rc override message +static uint32_t rc_override_fs_timer = 0; +// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal +static uint32_t ch3_failsafe_timer = 0; + +//////////////////////////////////////////////////////////////////////////////// +// LED output +//////////////////////////////////////////////////////////////////////////////// +// state of the GPS light (on/off) +static bool GPS_light; + +//////////////////////////////////////////////////////////////////////////////// +// GPS variables +//////////////////////////////////////////////////////////////////////////////// +// This is used to scale GPS values for EEPROM storage +// 10^7 times Decimal GPS means 1 == 1cm +// This approximation makes calculations integer and it's easy to read +static const float t7 = 10000000.0; +// We use atan2 and other trig techniques to calaculate angles +// We need to scale the longitude up to make these calcs work +// to account for decreasing distance between lines of longitude away from the equator +static float scaleLongUp = 1; +// Sometimes we need to remove the scaling for distance calcs +static float scaleLongDown = 1; +// A counter used to count down valid gps fixes to allow the gps estimate to settle +// before recording our home position (and executing a ground start if we booted with an air start) +static byte ground_start_count = 5; +// Used to compute a speed estimate from the first valid gps fixes to decide if we are +// on the ground or in the air. Used to decide if a ground start is appropriate if we +// booted with an air start. +static int ground_start_avg; +// Tracks if GPS is enabled based on statup routine +// If we do not detect GPS at startup, we stop trying and assume GPS is not connected +static bool GPS_enabled = false; +static int32_t gps_base_alt; + +//////////////////////////////////////////////////////////////////////////////// +// Location & Navigation +//////////////////////////////////////////////////////////////////////////////// +// Constants +const float radius_of_earth = 6378100; // meters +const float gravity = 9.81; // meters/ sec^2 +// This is the currently calculated direction to fly. +// deg * 100 : 0 to 360 +static long nav_bearing; +// This is the direction to the next waypoint or loiter center +// deg * 100 : 0 to 360 +static long target_bearing; +//This is the direction from the last waypoint to the next waypoint +// deg * 100 : 0 to 360 +static long crosstrack_bearing; +// A gain scaler to account for ground speed/headwind/tailwind +static float nav_gain_scaler = 1; +// Direction held during phases of takeoff and landing +// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use +static long hold_course = -1; // deg * 100 dir of plane + +// There may be two active commands in Auto mode. +// This indicates the active navigation command by index number +static byte nav_command_index; +// This indicates the active non-navigation command by index number +static byte non_nav_command_index; +// This is the command type (eg navigate to waypoint) of the active navigation command +static byte nav_command_ID = NO_COMMAND; +static byte non_nav_command_ID = NO_COMMAND; + +//////////////////////////////////////////////////////////////////////////////// +// Airspeed +//////////////////////////////////////////////////////////////////////////////// +// The current airspeed estimate/measurement in centimeters per second +static int airspeed; +// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met. +// Also used for flap deployment criteria. Centimeters per second.static long target_airspeed; +static long target_airspeed; +// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second. +static float airspeed_error; +static float groundspeed_error; +// The calculated total energy error (kinetic (altitude) plus potential (airspeed)). +// Used by the throttle controller +static long energy_error; +// kinetic portion of energy error (m^2/s^2) +static long airspeed_energy_error; +// An amount that the airspeed should be increased in auto modes based on the user positioning the +// throttle stick in the top half of the range. Centimeters per second. +static int airspeed_nudge; +// Similar to airspeed_nudge, but used when no airspeed sensor. +// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel +static int throttle_nudge = 0; + +//////////////////////////////////////////////////////////////////////////////// +// Ground speed +//////////////////////////////////////////////////////////////////////////////// +// The amount current ground speed is below min ground speed. Centimeters per second +static long groundspeed_undershoot = 0; +static long ground_speed = 0; + +//////////////////////////////////////////////////////////////////////////////// +// Location Errors +//////////////////////////////////////////////////////////////////////////////// +// Difference between current bearing and desired bearing. Hundredths of a degree +static long bearing_error; +// Difference between current altitude and desired altitude. Centimeters +static long altitude_error; +// Distance perpandicular to the course line that we are off trackline. Meters +static float crosstrack_error; + +//////////////////////////////////////////////////////////////////////////////// +// CH7 control +//////////////////////////////////////////////////////////////////////////////// + +// Used to track the CH7 toggle state. +// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true +// This allows advanced functionality to know when to execute +static boolean trim_flag; +// This register tracks the current Mission Command index when writing +// a mission using CH7 in flight +static int8_t CH7_wp_index; +float tuning_value; + +//////////////////////////////////////////////////////////////////////////////// +// Battery Sensors +//////////////////////////////////////////////////////////////////////////////// +// Battery pack 1 voltage. Initialized above the low voltage threshold to pre-load the filter and prevent low voltage events at startup. +static float battery_voltage1 = LOW_VOLTAGE * 1.05; +// Battery pack 1 instantaneous currrent draw. Amperes +static float current_amps1; +// Totalized current (Amp-hours) from battery 1 +static float current_total1; + +// To Do - Add support for second battery pack +//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter +//static float current_amps2; // Current (Amperes) draw from battery 2 +//static float current_total2; // Totalized current (Amp-hours) from battery 2 + +//////////////////////////////////////////////////////////////////////////////// +// Airspeed Sensors +//////////////////////////////////////////////////////////////////////////////// +// Raw differential pressure measurement (filtered). ADC units +static float airspeed_raw; +// Raw differential pressure less the zero pressure offset. ADC units +static float airspeed_pressure; +// The pressure at home location - calibrated at arming +static int32_t ground_pressure; +// The ground temperature at home location - calibrated at arming +static int16_t ground_temperature; +//////////////////////////////////////////////////////////////////////////////// +// Altitude Sensor variables +//////////////////////////////////////////////////////////////////////////////// +// Raw absolute pressure measurement (filtered). ADC units +static unsigned long abs_pressure; +// Altitude from the sonar sensor. Meters. Not yet implemented. +static int sonar_alt; +// The altitude as reported by Baro in cm – Values can be quite high +static int32_t baro_alt; + +//////////////////////////////////////////////////////////////////////////////// +// flight mode specific +//////////////////////////////////////////////////////////////////////////////// +// Flag for using gps ground course instead of IMU yaw. Set false when takeoff command in process. +static bool takeoff_complete = true; +// Flag to indicate if we have landed. +//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown +static bool land_complete; +// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters +static long takeoff_altitude; +// Pitch to hold during landing command in the no airspeed sensor case. Hundredths of a degree +static int landing_pitch; +// Minimum pitch to hold during takeoff command execution. Hundredths of a degree +static int takeoff_pitch; +static bool final = false; + +// JLN Update +unsigned long timesw = 0; +static long ground_course = 0; // deg * 100 dir of plane + +//////////////////////////////////////////////////////////////////////////////// +// Loiter management +//////////////////////////////////////////////////////////////////////////////// +// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree +static long old_target_bearing; +// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees +static int loiter_total; +// The amount in degrees we have turned since recording old_target_bearing +static int loiter_delta; +// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees +static int loiter_sum; +// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds. +static long loiter_time; +// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds. +static int loiter_time_max; + +//////////////////////////////////////////////////////////////////////////////// +// Navigation control variables +//////////////////////////////////////////////////////////////////////////////// +// The instantaneous desired bank angle. Hundredths of a degree +static long nav_roll; +// The instantaneous desired pitch angle. Hundredths of a degree +static long nav_pitch; +// Calculated radius for the wp turn based on ground speed and max turn angle +static long wp_radius; +static long toff_yaw; // deg * 100 : yaw angle for takeoff +static long nav_yaw = 1; // deg * 100 : target yaw angle - used only for thermal hunt +static long altitude_estimate = 0; // for smoothing GPS output + +//////////////////////////////////////////////////////////////////////////////// +// Waypoint distances +//////////////////////////////////////////////////////////////////////////////// +// Distance between plane and next waypoint. Meters +static long wp_distance; +// Distance between previous and next waypoint. Meters +static long wp_totalDistance; + +static long max_dist_set; // used for HEADALT (LEO) + +//////////////////////////////////////////////////////////////////////////////// +// repeating event control +//////////////////////////////////////////////////////////////////////////////// +// Flag indicating current event type +static byte event_id; +// when the event was started in ms +static long event_timer; +// how long to delay the next firing of event in millis +static uint16_t event_delay; +// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles +static int event_repeat = 0; +// per command value, such as PWM for servos +static int event_value; +// the value used to cycle events (alternate value to event_value) +static int event_undo_value; + +//////////////////////////////////////////////////////////////////////////////// +// Conditional command +//////////////////////////////////////////////////////////////////////////////// +// A value used in condition commands (eg delay, change alt, etc.) +// For example in a change altitude command, it is the altitude to change to. +static long condition_value; +// A starting value used to check the status of a conditional command. +// For example in a delay command the condition_start records that start time for the delay +static long condition_start; +// A value used in condition commands. For example the rate at which to change altitude. +static int condition_rate; + +//////////////////////////////////////////////////////////////////////////////// +// 3D Location vectors +// Location structure defined in AP_Common +//////////////////////////////////////////////////////////////////////////////// +// The home location used for RTL. The location is set when we first get stable GPS lock +static struct Location home; +// Flag for if we have g_gps lock and have set the home location +static bool home_is_set; +// The location of the previous waypoint. Used for track following and altitude ramp calculations +static struct Location prev_WP; +// The plane's current location +static struct Location current_loc; +// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. +static struct Location next_WP; +// The location of the active waypoint in Guided mode. +static struct Location guided_WP; +static struct Location soarwp1_WP = {0,0,0}; // temp waypoint for Ridge Soaring +static struct Location soarwp2_WP = {0,0,0}; // temp waypoint for Ridge Soaring +// The location structure information from the Nav command being processed +static struct Location next_nav_command; +// The location structure information from the Non-Nav command being processed +static struct Location next_nonnav_command; + +//////////////////////////////////////////////////////////////////////////////// +// Altitude / Climb rate control +//////////////////////////////////////////////////////////////////////////////// +// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters +static long target_altitude; +// Altitude difference between previous and current waypoint. Centimeters +static long offset_altitude; + +//////////////////////////////////////////////////////////////////////////////// +// IMU variables +//////////////////////////////////////////////////////////////////////////////// +// The main loop execution time. Seconds +//This is the time between calls to the DCM algorithm and is the Integration time for the gyros. +static float G_Dt = 0.02; + +//////////////////////////////////////////////////////////////////////////////// +// Performance monitoring +//////////////////////////////////////////////////////////////////////////////// +// Timer used to accrue data and trigger recording of the performanc monitoring log message +static long perf_mon_timer; +// The maximum main loop execution time recorded in the current performance monitoring interval +static int G_Dt_max = 0; +// The number of gps fixes recorded in the current performance monitoring interval +static int gps_fix_count = 0; +// A variable used by developers to track performanc metrics. +// Currently used to record the number of GCS heartbeat messages received +static int pmTest1 = 0; + + +//////////////////////////////////////////////////////////////////////////////// +// System Timers +//////////////////////////////////////////////////////////////////////////////// +// Time in miliseconds of start of main control loop. Milliseconds +static unsigned long fast_loopTimer; +// Time Stamp when fast loop was complete. Milliseconds +static unsigned long fast_loopTimeStamp; +// Number of milliseconds used in last main loop cycle +static uint8_t delta_ms_fast_loop; +// Counter of main loop executions. Used for performance monitoring and failsafe processing +static uint16_t mainLoop_count; + +// Time in miliseconds of start of medium control loop. Milliseconds +static unsigned long medium_loopTimer; +// Counters for branching from main control loop to slower loops +static byte medium_loopCounter; +// Number of milliseconds used in last medium loop cycle +static uint8_t delta_ms_medium_loop; + +// Counters for branching from medium control loop to slower loops +static byte slow_loopCounter; +// Counter to trigger execution of very low rate processes +static byte superslow_loopCounter; +// Counter to trigger execution of 1 Hz processes +static byte counter_one_herz; + +// used to track the elapsed time for navigation PID integral terms +static unsigned long nav_loopTimer; +// Elapsed time since last call to navigation pid functions +static unsigned long dTnav; +// % MCU cycles used +static float load; + +//////////////////////////////////////////////////////////////////////////////// +// Top-level logic +//////////////////////////////////////////////////////////////////////////////// + +void setup() { + memcheck_init(); + init_ardupilot(); +} + +void loop() +{ + // We want this to execute at 50Hz if possible + // ------------------------------------------- + if (millis()-fast_loopTimer > 19) { + delta_ms_fast_loop = millis() - fast_loopTimer; + load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop; + G_Dt = (float)delta_ms_fast_loop / 1000.f; + fast_loopTimer = millis(); + + mainLoop_count++; + + // Execute the fast loop + // --------------------- + fast_loop(); + + // Execute the medium loop + // ----------------------- + medium_loop(); + + counter_one_herz++; + if(counter_one_herz == 50){ + one_second_loop(); + counter_one_herz = 0; + } + + if (millis() - perf_mon_timer > 20000) { + if (mainLoop_count != 0) { + if (g.log_bitmask & MASK_LOG_PM) + #if HIL_MODE != HIL_MODE_ATTITUDE + Log_Write_Performance(); + #endif + + resetPerfData(); + } + } + + fast_loopTimeStamp = millis(); + } +} + +// Main loop 50Hz +static void fast_loop() +{ + // This is the fast loop - we want it to execute at 50Hz if possible + // ----------------------------------------------------------------- + if (delta_ms_fast_loop > G_Dt_max) + G_Dt_max = delta_ms_fast_loop; + + // Read radio + // ---------- + read_radio(); + + // try to send any deferred messages if the serial port now has + // some space available + gcs_send_message(MSG_RETRY_DEFERRED); + + // check for loss of control signal failsafe condition + // ------------------------------------ + check_short_failsafe(); + + #if HIL_MODE == HIL_MODE_SENSORS + // update hil before dcm update + gcs_update(); + #endif + + ahrs.update(); + + // uses the yaw from the DCM to give more accurate turns + calc_bearing_error(); + + # if HIL_MODE == HIL_MODE_DISABLED + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) + Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); + + if (g.log_bitmask & MASK_LOG_RAW) + Log_Write_Raw(); + #endif + + // inertial navigation + // ------------------ + #if INERTIAL_NAVIGATION == ENABLED + // TODO: implement inertial nav function + inertialNavigation(); + #endif + + // custom code/exceptions for flight modes + // --------------------------------------- + update_current_flight_mode(); + + // apply desired roll, pitch and yaw to the plane + // ---------------------------------------------- + if (control_mode > MANUAL) + stabilize(); + + // write out the servo PWM values + // ------------------------------ + set_servos(); + + + // XXX is it appropriate to be doing the comms below on the fast loop? + + gcs_update(); + gcs_data_stream_send(45,1000); +} + +static void medium_loop() +{ +#if MOUNT == ENABLED + camera_mount.update_mount_position(); +#endif + + // This is the start of the medium (10 Hz) loop pieces + // ----------------------------------------- + switch(medium_loopCounter) { + + // This case deals with the GPS + //------------------------------- + case 0: + medium_loopCounter++; + if(GPS_enabled){ + update_GPS(); + calc_gndspeed_undershoot(); + } + + #if HIL_MODE != HIL_MODE_ATTITUDE + if (g.compass_enabled && compass.read()) { + ahrs.set_compass(&compass); + // Calculate heading + Matrix3f m = ahrs.get_dcm_matrix(); + compass.calculate(m); + compass.null_offsets(); + } else { + ahrs.set_compass(NULL); + } + #endif +/*{ +Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); +Vector3f tempaccel = imu.get_accel(); +Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); +Serial.println(tempaccel.z, DEC); +}*/ + + break; + + // This case performs some navigation computations + //------------------------------------------------ + case 1: + medium_loopCounter++; + + + if(g_gps->new_data){ + g_gps->new_data = false; + dTnav = millis() - nav_loopTimer; + nav_loopTimer = millis(); + + // calculate the plane's desired bearing + // ------------------------------------- + navigate(); + } + + break; + + // command processing + //------------------------------ + case 2: + medium_loopCounter++; + + // perform next command + // -------------------- + update_commands(); + break; + + // This case deals with sending high rate telemetry + //------------------------------------------------- + case 3: + medium_loopCounter++; + + #if HIL_MODE != HIL_MODE_ATTITUDE + if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) + Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); + + if (g.log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); + #endif + + if (g.log_bitmask & MASK_LOG_NTUN) + Log_Write_Nav_Tuning(); + + if (g.log_bitmask & MASK_LOG_GPS) + Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); + + // send all requested output streams with rates requested + // between 5 and 45 Hz + gcs_data_stream_send(5,45); + break; + + // This case controls the slow loop + //--------------------------------- + case 4: + medium_loopCounter = 0; + delta_ms_medium_loop = millis() - medium_loopTimer; + medium_loopTimer = millis(); + + if (g.battery_monitoring != 0){ + read_battery(); + } + + read_trim_switch(); + + slow_loop(); + break; + } +} + +static void slow_loop() +{ + // This is the slow (3 1/3 Hz) loop pieces + //---------------------------------------- + switch (slow_loopCounter){ + case 0: + slow_loopCounter++; + check_long_failsafe(); + superslow_loopCounter++; + if(superslow_loopCounter >=200) { // 200 = Execute every minute + #if HIL_MODE != HIL_MODE_ATTITUDE + if(g.compass_enabled) { + compass.save_offsets(); + } + #endif + + superslow_loopCounter = 0; + } + break; + + case 1: + slow_loopCounter++; + + // Read 3-position switch on radio + // ------------------------------- + read_control_switch(); + + // Read Control Surfaces/Mix switches + // ---------------------------------- + update_servo_switches(); + + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); + +#if MOUNT == ENABLED + camera_mount.update_mount_type(); +#endif + break; + + case 2: + slow_loopCounter = 0; + + update_events(); + + mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter + gcs_data_stream_send(3,5); + +#if USB_MUX_PIN > 0 + check_usb_mux(); +#endif + break; + } +} + +static void one_second_loop() +{ + if (g.log_bitmask & MASK_LOG_CUR) + Log_Write_Current(); + + // send a heartbeat + gcs_send_message(MSG_HEARTBEAT); + gcs_data_stream_send(1,3); +} + +static void update_GPS(void) +{ + g_gps->update(); + update_GPS_light(); + + if (g_gps->new_data && g_gps->fix) { + // for performance + // --------------- + gps_fix_count++; + + if(ground_start_count > 1){ + ground_start_count--; + ground_start_avg += g_gps->ground_speed; + + } else if (ground_start_count == 1) { + // We countdown N number of good GPS fixes + // so that the altitude is more accurate + // ------------------------------------- + if (current_loc.lat == 0) { + ground_start_count = 5; + + } else { + if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){ + startup_ground(); + + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + + init_home(); + } else if (ENABLE_AIR_START == 0) { + init_home(); + } + + if (g.compass_enabled) { + // Set compass declination automatically + compass.set_initial_location(g_gps->latitude, g_gps->longitude); + } + ground_start_count = 0; + } + } + + + current_loc.lng = g_gps->longitude; // Lon * 10**7 + current_loc.lat = g_gps->latitude; // Lat * 10**7 + current_loc.alt = max((g_gps->altitude - home.alt),0); + ground_course = g_gps->ground_course; + ground_speed = g_gps->ground_speed; + + // see if we've breached the geo-fence + geofence_check(false); + } +} + +static void update_current_flight_mode(void) +{ int AOAstart; + if(control_mode == AUTO){ + + switch(nav_command_ID){ + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_LAND: + break; + default: + hold_course = -1; + calc_nav_roll(); + calc_throttle(); + break; + } + }else{ + switch(control_mode){ + case RTL: + case LOITER: + case GUIDED: + hold_course = -1; + calc_nav_roll(); + calc_throttle(); + break; + + case FLY_BY_WIRE_A: + case FLY_BY_WIRE_B: + break; + + case STABILIZE: + nav_roll = 0; + nav_pitch = 0; + // throttle is passthrough + break; + + case CIRCLE: + // we have no GPS installed and have lost radio contact + // or we just want to fly around in a gentle circle w/o GPS + // ---------------------------------------------------- + nav_roll = g.roll_limit / 3; + nav_pitch = 0; + + if (failsafe != FAILSAFE_NONE){ + g.channel_throttle.servo_out = g.throttle_cruise; + } + break; + + case MANUAL: + // servo_out is for Sim control only + // --------------------------------- + g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); + g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); + g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); + break; + //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 + + } + } +} + +static void update_navigation() +{ + // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS + // ------------------------------------------------------------------------ + + // distance and bearing calcs only + if(control_mode == AUTO){ + verify_commands(); + }else{ + + switch(control_mode){ + case LOITER: + case RTL: + case GUIDED: + update_loiter(); + calc_bearing_error(); + break; + + } + } +} diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde new file mode 100644 index 0000000000..5a31fd23c2 --- /dev/null +++ b/APMrover2/Attitude.pde @@ -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--; + } +} diff --git a/APMrover2/GCS.h b/APMrover2/GCS.h new file mode 100644 index 0000000000..7047beaf09 --- /dev/null +++ b/APMrover2/GCS.h @@ -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 +#include +#include +#include +#include +#include + +/// +/// @class GCS +/// @brief Class describing the interface between the APM code +/// proper and the GCS implementation. +/// +/// GCS' are currently implemented inside the sketch and as such have +/// access to all global state. The sketch should not, however, call GCS +/// internal functions - all calls to the GCS should be routed through +/// this interface (or functions explicitly exposed by a subclass). +/// +class GCS_Class +{ +public: + + /// Startup initialisation. + /// + /// This routine performs any one-off initialisation required before + /// GCS messages are exchanged. + /// + /// @note The stream is expected to be set up and configured for the + /// correct bitrate before ::init is called. + /// + /// @note The stream is currently BetterStream so that we can use the _P + /// methods; this may change if Arduino adds them to Print. + /// + /// @param port The stream over which messages are exchanged. + /// + void init(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 diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde new file mode 100644 index 0000000000..53fd2a7a45 --- /dev/null +++ b/APMrover2/GCS_Mavlink.pde @@ -0,0 +1,2181 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Mavlink_compat.h" + +// use this to prevent recursion during sensor init +static bool in_mavlink_delay; + +// this costs us 51 bytes, but means that low priority +// messages don't block the CPU +static mavlink_statustext_t pending_status; + +// true when we have received at least 1 MAVLink packet +static bool mavlink_active; + +// check if a message will fit in the payload space available +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ +#ifdef MAVLINK10 + uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + uint8_t system_status = MAV_STATE_ACTIVE; + uint32_t custom_mode = control_mode; + + // work out the base_mode. This value is not very useful + // for APM, but we calculate it as best we can so a generic + // MAVLink enabled ground station can work out something about + // what the MAV is up to. The actual bit values are highly + // ambiguous for most of the APM flight modes. In practice, you + // only get useful information from the custom_mode, which maps to + // the APM flight mode and has a well defined meaning in the + // ArduPlane documentation + switch (control_mode) { + case MANUAL: + base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + break; + case STABILIZE: + case FLY_BY_WIRE_A: + case FLY_BY_WIRE_B: + case FLY_BY_WIRE_C: + base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; + break; + case AUTO: + case RTL: + case LOITER: + case GUIDED: + case CIRCLE: + base_mode = MAV_MODE_FLAG_GUIDED_ENABLED | + MAV_MODE_FLAG_STABILIZE_ENABLED; + // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what + // APM does in any mode, as that is defined as "system finds its own goal + // positions", which APM does not currently do + break; + case INITIALISING: + system_status = MAV_STATE_CALIBRATING; + break; + } + + if (control_mode != MANUAL && control_mode != INITIALISING) { + // stabiliser of some form is enabled + base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + +#if ENABLE_STICK_MIXING==ENABLED + if (control_mode != INITIALISING) { + // all modes except INITIALISING have some form of manual + // override if stick mixing is enabled + base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } +#endif + +#if HIL_MODE != HIL_MODE_DISABLED + base_mode |= MAV_MODE_FLAG_HIL_ENABLED; +#endif + + // we are armed if we are not initialising + if (control_mode != INITIALISING) { + base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + // indicate we have set a custom mode + base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + mavlink_msg_heartbeat_send( + chan, + MAV_TYPE_FIXED_WING, + MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode, + custom_mode, + system_status); +#else // MAVLINK10 + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +#endif // MAVLINK10 +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + Vector3f omega = ahrs.get_gyro(); + mavlink_msg_attitude_send( + chan, + micros(), + ahrs.roll, + ahrs.pitch - radians(g.pitch_trim*0.01), + ahrs.yaw, + omega.x, + omega.y, + omega.z); +} + +#if GEOFENCE_ENABLED == ENABLED +static NOINLINE void send_fence_status(mavlink_channel_t chan) +{ + geofence_send_status(chan); +} +#endif + + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ +#ifdef MAVLINK10 + uint32_t control_sensors_present = 0; + uint32_t control_sensors_enabled; + uint32_t control_sensors_health; + + // first what sensors/controllers we have + control_sensors_present |= (1<<0); // 3D gyro present + control_sensors_present |= (1<<1); // 3D accelerometer present + if (g.compass_enabled) { + control_sensors_present |= (1<<2); // compass present + } + control_sensors_present |= (1<<3); // absolute pressure sensor present + if (g_gps->fix) { + control_sensors_present |= (1<<5); // GPS present + } + control_sensors_present |= (1<<10); // 3D angular rate control + control_sensors_present |= (1<<11); // attitude stabilisation + control_sensors_present |= (1<<12); // yaw position + control_sensors_present |= (1<<13); // altitude control + control_sensors_present |= (1<<14); // X/Y position control + control_sensors_present |= (1<<15); // motor control + + // now what sensors/controllers are enabled + + // first the sensors + control_sensors_enabled = control_sensors_present & 0x1FF; + + // now the controllers + control_sensors_enabled = control_sensors_present & 0x1FF; + + switch (control_mode) { + case MANUAL: + break; + + case STABILIZE: + case FLY_BY_WIRE_A: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + break; + + case FLY_BY_WIRE_B: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + control_sensors_enabled |= (1<<15); // motor control + break; + + case FLY_BY_WIRE_C: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + control_sensors_enabled |= (1<<13); // altitude control + control_sensors_enabled |= (1<<15); // motor control + break; + + case AUTO: + case RTL: + case LOITER: + case GUIDED: + case CIRCLE: + control_sensors_enabled |= (1<<10); // 3D angular rate control + control_sensors_enabled |= (1<<11); // attitude stabilisation + control_sensors_enabled |= (1<<12); // yaw position + control_sensors_enabled |= (1<<13); // altitude control + control_sensors_enabled |= (1<<14); // X/Y position control + control_sensors_enabled |= (1<<15); // motor control + break; + + case INITIALISING: + break; + } + + // at the moment all sensors/controllers are assumed healthy + control_sensors_health = control_sensors_present; + + uint16_t battery_current = -1; + uint8_t battery_remaining = -1; + + if (current_total1 != 0 && g.pack_capacity != 0) { + battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity); + } + if (current_total1 != 0) { + battery_current = current_amps1 * 100; + } + + if (g.battery_monitoring == 3) { + /*setting a out-of-range value. + It informs to external devices that + it cannot be calculated properly just by voltage*/ + battery_remaining = 150; + } + + mavlink_msg_sys_status_send( + chan, + control_sensors_present, + control_sensors_enabled, + control_sensors_health, + (uint16_t)(load * 1000), + battery_voltage1 * 1000, // mV + battery_current, // in 10mA units + battery_remaining, // in % + 0, // comm drops %, + 0, // comm drops in pkts, + 0, 0, 0, 0); + +#else // MAVLINK10 + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case MANUAL: + mode = MAV_MODE_MANUAL; + break; + case STABILIZE: + mode = MAV_MODE_TEST1; + break; + case FLY_BY_WIRE_A: + mode = MAV_MODE_TEST2; + nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case FLY_BY_WIRE_B: + mode = MAV_MODE_TEST2; + nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_LOITER; + break; + case INITIALISING: + mode = MAV_MODE_UNINIT; + nav_mode = MAV_NAV_GROUNDED; + break; + case CIRCLE: + mode = MAV_MODE_TEST3; + break; + } + + uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + if (g.battery_monitoring == 3) { + /*setting a out-of-range value. + It informs to external devices that + it cannot be calculated properly just by voltage*/ + battery_remaining = 1500; + } + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + load * 1000, + battery_voltage1 * 1000, + battery_remaining, + packet_drops); +#endif // MAVLINK10 +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + millis(), + current_loc.lat, // in 1E7 degrees + current_loc.lng, // in 1E7 degrees + g_gps->altitude*10, // millimeters above sea level + current_loc.alt * 10, // millimeters above ground + g_gps->ground_speed * rot.a.x, // X speed cm/s + g_gps->ground_speed * rot.b.x, // Y speed cm/s + g_gps->ground_speed * rot.c.x, + g_gps->ground_course); // course in 1/100 degree +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + int16_t bearing = (hold_course==-1?nav_bearing:hold_course) / 100; + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + bearing, + target_bearing / 100, + wp_distance, + altitude_error / 1.0e2, + airspeed_error, + crosstrack_error); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ +#ifdef MAVLINK10 + uint8_t fix; + if (g_gps->status() == 2) { + fix = 3; + } else { + fix = 0; + } + + mavlink_msg_gps_raw_int_send( + chan, + micros(), + fix, + g_gps->latitude, // in 1E7 degrees + g_gps->longitude, // in 1E7 degrees + g_gps->altitude * 10, // in mm + g_gps->hdop, + 65535, + g_gps->ground_speed, // cm/s + g_gps->ground_course, // 1/100 degrees, + g_gps->num_sats); + +#else // MAVLINK10 + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +#endif // MAVLINK10 +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with + // HIL maintainers +#if X_PLANE == ENABLED + /* update by JLN for X-Plane or AeroSIM HIL */ + + int thr_out = constrain((g.channel_throttle.servo_out *2) - 100, -100, 100); // throttle set from -100 to 100 + mavlink_msg_rc_channels_scaled_send( + chan, + g.channel_roll.servo_out, + g.channel_pitch.servo_out, + 100 * thr_out, + g.channel_rudder.servo_out, + 10000 * g.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + rssi); +#else + mavlink_msg_rc_channels_scaled_send( + chan, + millis(), + 0, // port 0 + 10000 * g.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + 0, + 0, + 0, + 0, + rssi); +#endif +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + millis(), + 0, // port + 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, // XXX currently only 4 RC channels defined + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + micros(), + 0, // port + g.channel_roll.radio_out, + g.channel_pitch.radio_out, + g.channel_throttle.radio_out, + g.channel_rudder.radio_out, + g.rc_5.radio_out, // XXX currently only 4 RC channels defined + g.rc_6.radio_out, + g.rc_7.radio_out, + g.rc_8.radio_out); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (ahrs.yaw_sensor / 100) % 360, + (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100 + current_loc.alt / 100.0, + 0); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + int32_t pressure = barometer.get_pressure(); + mavlink_msg_scaled_pressure_send( + chan, + micros(), + pressure/100.0, + (pressure - g.ground_pressure)/100.0, + barometer.get_temperature()); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + Vector3f mag_offsets = compass.get_offsets(); + + mavlink_msg_sensor_offsets_send(chan, + mag_offsets.x, + mag_offsets.y, + mag_offsets.z, + compass.get_declination(), + barometer.get_raw_pressure(), + barometer.get_raw_temp(), + imu.gx(), imu.gy(), imu.gz(), + imu.ax(), imu.ay(), imu.az()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.command_index); +} + +static void NOINLINE send_statustext(mavlink_channel_t chan) +{ + mavlink_msg_statustext_send( + chan, + pending_status.severity, + pending_status.text); +} + + +// try to send a message, return false if it won't fit in the serial tx buffer +static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) +{ + int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; + + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // defer any messages on the telemetry port for 1 second after + // bootup, to try to prevent bricking of Xbees + return false; + } + + switch (id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + return true; + + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; + + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; + + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; + + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; + + case MSG_NAV_CONTROLLER_OUTPUT: + if (control_mode != MANUAL) { + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); + } + break; + + case MSG_GPS_RAW: +#ifdef MAVLINK10 + CHECK_PAYLOAD_SIZE(GPS_RAW_INT); +#else + CHECK_PAYLOAD_SIZE(GPS_RAW); +#endif + send_gps_raw(chan); + break; + + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; + + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; + + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; + + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; + +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; + + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; + + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE + + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; + + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; + + case MSG_NEXT_PARAM: + CHECK_PAYLOAD_SIZE(PARAM_VALUE); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_param_send(); + } else if (gcs3.initialised) { + gcs3.queued_param_send(); + } + break; + + case MSG_NEXT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_waypoint_send(); + } else if (gcs3.initialised) { + gcs3.queued_waypoint_send(); + } + break; + + case MSG_STATUSTEXT: + CHECK_PAYLOAD_SIZE(STATUSTEXT); + send_statustext(chan); + break; + +#if GEOFENCE_ENABLED == ENABLED + case MSG_FENCE_STATUS: + CHECK_PAYLOAD_SIZE(FENCE_STATUS); + send_fence_status(chan); + break; +#endif + + case MSG_RETRY_DEFERRED: + break; // just here to prevent a warning + } + return true; +} + + +#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED +static struct mavlink_queue { + enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES]; + uint8_t next_deferred_message; + uint8_t num_deferred_messages; +} mavlink_queue[2]; + +// send a message using mavlink +static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) +{ + uint8_t i, nextid; + struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; + + // see if we can send the deferred messages, if any + while (q->num_deferred_messages != 0) { + if (!mavlink_try_send_message(chan, + q->deferred_messages[q->next_deferred_message], + packet_drops)) { + break; + } + q->next_deferred_message++; + if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { + q->next_deferred_message = 0; + } + q->num_deferred_messages--; + } + + if (id == MSG_RETRY_DEFERRED) { + return; + } + + // this message id might already be deferred + for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { + if (q->deferred_messages[nextid] == id) { + // its already deferred, discard + return; + } + nextid++; + if (nextid == MAX_DEFERRED_MESSAGES) { + nextid = 0; + } + } + + if (q->num_deferred_messages != 0 || + !mavlink_try_send_message(chan, id, packet_drops)) { + // can't send it now, so defer it + if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { + // the defer buffer is full, discard + return; + } + nextid = q->next_deferred_message + q->num_deferred_messages; + if (nextid >= MAX_DEFERRED_MESSAGES) { + nextid -= MAX_DEFERRED_MESSAGES; + } + q->deferred_messages[nextid] = id; + q->num_deferred_messages++; + } +} + +void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str) +{ + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // don't send status MAVLink messages for 2 seconds after + // bootup, to try to prevent Xbee bricking + return; + } + + if (severity == SEVERITY_LOW) { + // send via the deferred queuing system + pending_status.severity = (uint8_t)severity; + strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); + mavlink_send_message(chan, MSG_STATUSTEXT, 0); + } else { + // send immediately + mavlink_msg_statustext_send(chan, severity, str); + } +} + +const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRateRawSensors), + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus), + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRateRCChannels), + AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController), + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition), + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1), + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2), + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3), + AP_GROUPEND +}; + + +GCS_MAVLINK::GCS_MAVLINK() : + packet_drops(0), + waypoint_send_timeout(1000), // 1 second + waypoint_receive_timeout(1000) // 1 second +{ +} + +void +GCS_MAVLINK::init(FastSerial * port) +{ + GCS_Class::init(port); + if (port == &Serial) { + mavlink_comm_0_port = port; + chan = MAVLINK_COMM_0; + }else{ + mavlink_comm_1_port = port; + chan = MAVLINK_COMM_1; + } + _queued_parameter = NULL; +} + +void +GCS_MAVLINK::update(void) +{ + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + status.packet_rx_drop_count = 0; + + // process received bytes + while (comm_get_available(chan)) + { + uint8_t c = comm_receive_ch(chan); + +#if CLI_ENABLED == ENABLED + /* allow CLI to be started by hitting enter 3 times, if no + heartbeat packets have been received */ + if (mavlink_active == 0 && millis() < 20000) { + if (c == '\n' || c == '\r') { + crlf_count++; + } else { + crlf_count = 0; + } + if (crlf_count == 3) { + run_cli(); + } + } +#endif + + // Try to get a new message + if (mavlink_parse_char(chan, c, &msg, &status)) { + mavlink_active = 1; + handleMessage(&msg); + } + } + + // Update packet drops counter + packet_drops += status.packet_rx_drop_count; + + // send out queued params/ waypoints + if (NULL != _queued_parameter) { + send_message(MSG_NEXT_PARAM); + } + + if (!waypoint_receiving && !waypoint_sending) { + return; + } + + uint32_t tnow = millis(); + + if (waypoint_receiving && + waypoint_request_i <= (unsigned)g.command_total && + tnow > waypoint_timelast_request + 500) { + waypoint_timelast_request = tnow; + send_message(MSG_NEXT_WAYPOINT); + } + + // stop waypoint sending if timeout + if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ + waypoint_sending = false; + } + + // stop waypoint receiving if timeout + if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ + waypoint_receiving = false; + } +} + +void +GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) +{ + if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { + + if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ + send_message(MSG_RAW_IMU1); + send_message(MSG_RAW_IMU2); + send_message(MSG_RAW_IMU3); + } + + if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { + send_message(MSG_EXTENDED_STATUS1); + send_message(MSG_EXTENDED_STATUS2); + send_message(MSG_GPS_STATUS); + send_message(MSG_CURRENT_WAYPOINT); + send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working + send_message(MSG_NAV_CONTROLLER_OUTPUT); + send_message(MSG_FENCE_STATUS); + } + + if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { + // sent with GPS read + send_message(MSG_LOCATION); + } + + if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { + // This is used for HIL. Do not change without discussing with HIL maintainers + send_message(MSG_SERVO_OUT); + } + + if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { + send_message(MSG_RADIO_OUT); + send_message(MSG_RADIO_IN); + } + + if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info + send_message(MSG_ATTITUDE); + } + + if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info + send_message(MSG_VFR_HUD); + } + + if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ + // Available datastream + } + } +} + + + +void +GCS_MAVLINK::send_message(enum ap_message id) +{ + mavlink_send_message(chan,id, packet_drops); +} + +void +GCS_MAVLINK::send_text(gcs_severity severity, const char *str) +{ + mavlink_send_text(chan,severity,str); +} + +void +GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) +{ + mavlink_statustext_t m; + uint8_t i; + for (i=0; imsgid) { + + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + { + // decode + mavlink_request_data_stream_t packet; + mavlink_msg_request_data_stream_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + int freq = 0; // packet frequency + + if (packet.start_stop == 0) + freq = 0; // stop sending + else if (packet.start_stop == 1) + freq = packet.req_message_rate; // start sending + else + break; + + switch(packet.req_stream_id){ + + case MAV_DATA_STREAM_ALL: + streamRateRawSensors.set_and_save_ifchanged(freq); + streamRateExtendedStatus.set_and_save_ifchanged(freq); + streamRateRCChannels.set_and_save_ifchanged(freq); + streamRateRawController.set_and_save_ifchanged(freq); + streamRatePosition.set_and_save_ifchanged(freq); + streamRateExtra1.set_and_save_ifchanged(freq); + streamRateExtra2.set_and_save_ifchanged(freq); + streamRateExtra3.set_and_save_ifchanged(freq); + break; + + case MAV_DATA_STREAM_RAW_SENSORS: + streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly + // we will not continue to broadcast raw sensor data at 50Hz. + break; + case MAV_DATA_STREAM_EXTENDED_STATUS: + streamRateExtendedStatus.set_and_save_ifchanged(freq); + break; + + case MAV_DATA_STREAM_RC_CHANNELS: + streamRateRCChannels.set_and_save_ifchanged(freq); + break; + + case MAV_DATA_STREAM_RAW_CONTROLLER: + streamRateRawController.set_and_save_ifchanged(freq); + break; + + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; + + case MAV_DATA_STREAM_POSITION: + streamRatePosition.set_and_save_ifchanged(freq); + break; + + case MAV_DATA_STREAM_EXTRA1: + streamRateExtra1.set_and_save_ifchanged(freq); + break; + + case MAV_DATA_STREAM_EXTRA2: + streamRateExtra2.set_and_save_ifchanged(freq); + break; + + case MAV_DATA_STREAM_EXTRA3: + streamRateExtra3.set_and_save_ifchanged(freq); + break; + + default: + break; + } + break; + } + +#ifdef MAVLINK10 + case MAVLINK_MSG_ID_COMMAND_LONG: + { + // decode + mavlink_command_long_t packet; + mavlink_msg_command_long_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; + + uint8_t result; + + // do command + send_text(SEVERITY_LOW,PSTR("command received: ")); + + switch(packet.command) { + + case MAV_CMD_NAV_LOITER_UNLIM: + set_mode(LOITER); + result = MAV_RESULT_ACCEPTED; + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + set_mode(RTL); + result = MAV_RESULT_ACCEPTED; + break; + +#if 0 + // not implemented yet, but could implement some of them + case MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_ROI: + case MAV_CMD_NAV_PATHPLANNING: + break; +#endif + + + case MAV_CMD_PREFLIGHT_CALIBRATION: + if (packet.param1 == 1 || + packet.param2 == 1 || + packet.param3 == 1) { + startup_IMU_ground(false); + } + if (packet.param4 == 1) { + trim_radio(); + } + result = MAV_RESULT_ACCEPTED; + break; + + + default: + result = MAV_RESULT_UNSUPPORTED; + break; + } + + mavlink_msg_command_ack_send( + chan, + packet.command, + result); + + break; + } + +#else // MAVLINK10 + case MAVLINK_MSG_ID_ACTION: + { + // decode + mavlink_action_t packet; + mavlink_msg_action_decode(msg, &packet); + if (mavlink_check_target(packet.target,packet.target_component)) break; + + uint8_t result = 0; + + // do action + send_text(SEVERITY_LOW,PSTR("action received: ")); +//Serial.println(packet.action); + switch(packet.action){ + + case MAV_ACTION_LAUNCH: + //set_mode(TAKEOFF); + break; + + case MAV_ACTION_RETURN: + set_mode(RTL); + result=1; + break; + + case MAV_ACTION_EMCY_LAND: + //set_mode(LAND); + break; + + case MAV_ACTION_HALT: + do_loiter_at_location(); + result=1; + break; + + /* No mappable implementation in APM 2.0 + case MAV_ACTION_MOTORS_START: + case MAV_ACTION_CONFIRM_KILL: + case MAV_ACTION_EMCY_KILL: + case MAV_ACTION_MOTORS_STOP: + case MAV_ACTION_SHUTDOWN: + break; + */ + + case MAV_ACTION_CONTINUE: + process_next_command(); + result=1; + break; + + case MAV_ACTION_SET_MANUAL: + set_mode(MANUAL); + result=1; + break; + + case MAV_ACTION_SET_AUTO: + set_mode(AUTO); + result=1; + // clearing failsafe should not be needed + // here. Added based on some puzzling results in + // the simulator (tridge) + failsafe = FAILSAFE_NONE; + break; + + case MAV_ACTION_STORAGE_READ: + // we load all variables at startup, and + // save on each mavlink set + result=1; + break; + + case MAV_ACTION_STORAGE_WRITE: + // this doesn't make any sense, as we save + // all settings as they come in + result=1; + break; + + case MAV_ACTION_CALIBRATE_RC: //break; + trim_radio(); + result=1; + break; + + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_ACC: + case MAV_ACTION_CALIBRATE_PRESSURE: + case MAV_ACTION_REBOOT: // this is a rough interpretation + startup_IMU_ground(true); + result=1; + break; + + /* For future implemtation + case MAV_ACTION_REC_START: break; + case MAV_ACTION_REC_PAUSE: break; + case MAV_ACTION_REC_STOP: break; + */ + + /* Takeoff is not an implemented flight mode in APM 2.0 + case MAV_ACTION_TAKEOFF: + set_mode(TAKEOFF); + break; + */ + + case MAV_ACTION_NAVIGATE: + set_mode(AUTO); + result=1; + break; + + /* Land is not an implemented flight mode in APM 2.0 + case MAV_ACTION_LAND: + set_mode(LAND); + break; + */ + + case MAV_ACTION_LOITER: + set_mode(LOITER); + result=1; + break; + + default: break; + } + + mavlink_msg_action_ack_send( + chan, + packet.action, + result + ); + + break; + } +#endif + + case MAVLINK_MSG_ID_SET_MODE: + { + // decode + mavlink_set_mode_t packet; + mavlink_msg_set_mode_decode(msg, &packet); + +#ifdef MAVLINK10 + if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) { + // we ignore base_mode as there is no sane way to map + // from that bitmap to a APM flight mode. We rely on + // custom_mode instead. + break; + } + switch (packet.custom_mode) { + case MANUAL: + case CIRCLE: + case STABILIZE: + case FLY_BY_WIRE_A: + case FLY_BY_WIRE_B: + case FLY_BY_WIRE_C: + case AUTO: + case RTL: + case LOITER: + set_mode(packet.custom_mode); + break; + } + +#else // MAVLINK10 + + switch(packet.mode){ + + case MAV_MODE_MANUAL: + set_mode(MANUAL); + break; + + case MAV_MODE_GUIDED: + set_mode(GUIDED); + break; + + case MAV_MODE_AUTO: + if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); + if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); + if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); + mav_nav = 255; + break; + + case MAV_MODE_TEST1: + set_mode(STABILIZE); + break; + + case MAV_MODE_TEST2: + if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A); + if(mav_nav == 2) set_mode(FLY_BY_WIRE_B); + //if(mav_nav == 3) set_mode(FLY_BY_WIRE_C); + mav_nav = 255; + break; + + } +#endif + break; + } + +#ifndef MAVLINK10 + case MAVLINK_MSG_ID_SET_NAV_MODE: + { + // decode + mavlink_set_nav_mode_t packet; + mavlink_msg_set_nav_mode_decode(msg, &packet); + // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message + mav_nav = packet.nav_mode; + break; + } +#endif // MAVLINK10 + + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + // decode + mavlink_waypoint_request_list_t packet; + mavlink_msg_waypoint_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // Start sending waypoints + mavlink_msg_waypoint_count_send( + chan,msg->sysid, + msg->compid, + g.command_total + 1); // + home + + waypoint_timelast_send = millis(); + waypoint_sending = true; + waypoint_receiving = false; + waypoint_dest_sysid = msg->sysid; + waypoint_dest_compid = msg->compid; + break; + } + + + // XXX read a WP from EEPROM and send it to the GCS + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + // Check if sending waypiont + //if (!waypoint_sending) break; + // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW + + // decode + mavlink_waypoint_request_t packet; + mavlink_msg_waypoint_request_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // send waypoint + tell_command = get_cmd_with_index(packet.seq); + + // set frame of waypoint + uint8_t frame; + + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame + } else { + frame = MAV_FRAME_GLOBAL; // reference frame + } + + float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; + + // time that the mav should loiter in milliseconds + uint8_t current = 0; // 1 (true), 0 (false) + + if (packet.seq == (uint16_t)g.command_index) + current = 1; + + uint8_t autocontinue = 1; // 1 (true), 0 (false) + + float x = 0, y = 0, z = 0; + + if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { + // command needs scaling + x = tell_command.lat/1.0e7; // local (x), global (latitude) + y = tell_command.lng/1.0e7; // local (y), global (longitude) + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt + } else { + z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) + } + } + + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_DO_SET_HOME: + param1 = tell_command.p1; + break; + + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1*10; // APM loiter time is in ten second increments + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + x=0; // Clear fields loaded above that we don't want sent for this command + y=0; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; + + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; + + case MAV_CMD_DO_REPEAT_SERVO: + param4 = tell_command.lng; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + param3 = tell_command.lat; + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + } + + mavlink_msg_waypoint_send(chan,msg->sysid, + msg->compid, + packet.seq, + frame, + tell_command.id, + current, + autocontinue, + param1, + param2, + param3, + param4, + x, + y, + z); + + // update last waypoint comm stamp + waypoint_timelast_send = millis(); + break; + } + + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + // decode + mavlink_waypoint_ack_t packet; + mavlink_msg_waypoint_ack_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // turn off waypoint send + waypoint_sending = false; + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + { + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // Start sending parameters - next call to ::update will kick the first one out + + _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type); + _queued_parameter_index = 0; + _queued_parameter_count = _count_parameters(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + // decode + mavlink_waypoint_clear_all_t packet; + mavlink_msg_waypoint_clear_all_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; + + // clear all commands + g.command_total.set_and_save(0); + + // note that we don't send multiple acks, as otherwise a + // GCS that is doing a clear followed by a set may see + // the additional ACKs as ACKs of the set operations + mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + // decode + mavlink_waypoint_set_current_t packet; + mavlink_msg_waypoint_set_current_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // set current command + change_command(packet.seq); + + mavlink_msg_waypoint_current_send(chan, g.command_index); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + // decode + mavlink_waypoint_count_t packet; + mavlink_msg_waypoint_count_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // start waypoint receiving + if (packet.count > MAX_WAYPOINTS) { + packet.count = MAX_WAYPOINTS; + } + g.command_total.set_and_save(packet.count - 1); + + waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; + waypoint_receiving = true; + waypoint_sending = false; + waypoint_request_i = 0; + break; + } + +#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS + case MAVLINK_MSG_ID_SET_MAG_OFFSETS: + { + mavlink_set_mag_offsets_t packet; + mavlink_msg_set_mag_offsets_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); + break; + } +#endif + + // XXX receive a WP from GCS and store in EEPROM + case MAVLINK_MSG_ID_WAYPOINT: + { + // decode + mavlink_waypoint_t packet; + uint8_t result = MAV_MISSION_ACCEPTED; + + mavlink_msg_waypoint_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // defaults + tell_command.id = packet.command; + + switch (packet.frame) + { + case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: + { + tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.options = 0; // absolute altitude + break; + } + +#ifdef MAV_FRAME_LOCAL_NED + case MAV_FRAME_LOCAL_NED: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = -packet.z*1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } +#endif + +#ifdef MAV_FRAME_LOCAL + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } +#endif + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + { + tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + break; + } + + default: + result = MAV_MISSION_UNSUPPORTED_FRAME; + break; + } + + + if (result != MAV_MISSION_ACCEPTED) goto mission_failed; + + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + case MAV_CMD_NAV_WAYPOINT: + case MAV_CMD_NAV_LOITER_UNLIM: + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + case MAV_CMD_NAV_LAND: + break; + + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_DO_SET_HOME: + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + tell_command.lat = packet.param1; + break; + + case MAV_CMD_NAV_LOITER_TIME: + tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments + break; + + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + tell_command.lat = packet.param1; + break; + + case MAV_CMD_DO_JUMP: + tell_command.lat = packet.param2; + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_DO_REPEAT_SERVO: + tell_command.lng = packet.param4; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + tell_command.lat = packet.param3; + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; + break; + + default: +#ifdef MAVLINK10 + result = MAV_MISSION_UNSUPPORTED; +#endif + break; + } + + if (result != MAV_MISSION_ACCEPTED) goto mission_failed; + + if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission + guided_WP = tell_command; + + // add home alt if needed + if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ + guided_WP.alt += home.alt; + } + + set_mode(GUIDED); + + // make any new wp uploaded instant (in case we are already in Guided mode) + set_guided_WP(); + + // verify we recevied the command + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + 0); + + } else { + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) { + result = MAV_MISSION_ERROR; + goto mission_failed; + } + + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) { + result = MAV_MISSION_INVALID_SEQUENCE; + goto mission_failed; + } + + set_cmd_with_index(tell_command, packet.seq); + + // update waypoint receiving state machine + waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; + waypoint_request_i++; + + if (waypoint_request_i > (uint16_t)g.command_total){ + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + result); + + send_text(SEVERITY_LOW,PSTR("flight plan received")); + waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } + } + break; + + mission_failed: + // we are rejecting the mission/waypoint + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + result); + break; + } + +#if GEOFENCE_ENABLED == ENABLED + // receive a fence point from GCS and store in EEPROM + case MAVLINK_MSG_ID_FENCE_POINT: { + mavlink_fence_point_t packet; + mavlink_msg_fence_point_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + if (g.fence_action != FENCE_ACTION_NONE) { + send_text(SEVERITY_LOW,PSTR("fencing must be disabled")); + } else if (packet.count != g.fence_total) { + send_text(SEVERITY_LOW,PSTR("bad fence point")); + } else { + Vector2l point; + point.x = packet.lat*1.0e7; + point.y = packet.lng*1.0e7; + set_fence_point_with_index(point, packet.idx); + } + break; + } + + // send a fence point to GCS + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { + mavlink_fence_fetch_point_t packet; + mavlink_msg_fence_fetch_point_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + if (packet.idx >= g.fence_total) { + send_text(SEVERITY_LOW,PSTR("bad fence point")); + } else { + Vector2l point = get_fence_point_with_index(packet.idx); + mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, g.fence_total, + point.x*1.0e-7, point.y*1.0e-7); + } + break; + } +#endif // GEOFENCE_ENABLED + + case MAVLINK_MSG_ID_PARAM_SET: + { + AP_Param *vp; + enum ap_var_type var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[ONBOARD_PARAM_NAME_LENGTH+1]; + strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); + key[ONBOARD_PARAM_NAME_LENGTH] = 0; + + // find the requested parameter + vp = AP_Param::find(key, &var_type); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + float rounding_addition = 0.01; + + // handle variables with standard type IDs + if (var_type == AP_PARAM_FLOAT) { + ((AP_Float *)vp)->set_and_save(packet.param_value); + } else if (var_type == AP_PARAM_INT32) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_PARAM_INT16) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_PARAM_INT8) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type + mavlink_msg_param_value_send( + chan, + key, + vp->cast_to_float(var_type), + mav_var_type(var_type), + _count_parameters(), + -1); // XXX we don't actually know what its index is... + } + + break; + } // end case + + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + { + // allow override of RC channel values for HIL + // or for complete GCS control of switch position + // and RC PWM values. + if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_rc_channels_override_t packet; + int16_t v[8]; + mavlink_msg_rc_channels_override_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system,packet.target_component)) + break; + + v[0] = packet.chan1_raw; + v[1] = packet.chan2_raw; + v[2] = packet.chan3_raw; + v[3] = packet.chan4_raw; + v[4] = packet.chan5_raw; + v[5] = packet.chan6_raw; + v[6] = packet.chan7_raw; + v[7] = packet.chan8_raw; + rc_override_active = APM_RC.setHIL(v); + rc_override_fs_timer = millis(); + break; + } + + case MAVLINK_MSG_ID_HEARTBEAT: + { + // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + if(msg->sysid != g.sysid_my_gcs) break; + rc_override_fs_timer = millis(); + pmTest1++; + break; + } + + #if HIL_MODE != HIL_MODE_DISABLED + // This is used both as a sensor and to pass the location + // in HIL_ATTITUDE mode. +#ifdef MAVLINK10 + case MAVLINK_MSG_ID_GPS_RAW_INT: + { + // decode + mavlink_gps_raw_int_t packet; + mavlink_msg_gps_raw_int_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.time_usec/1000.0, + packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, + packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 0); + break; + } +#else // MAVLINK10 + case MAVLINK_MSG_ID_GPS_RAW: + { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + packet.v,packet.hdg,0,0); + //if ((gps_base_alt == 0) && (airspeed ==0 )) { // we are on the ground so, get the altitude offset + // gps_base_alt = packet.alt*100; + //} + + current_loc.lng = packet.lon * T7; + current_loc.lat = packet.lat * T7; + //current_loc.alt = g_gps->altitude - gps_base_alt; + //if (!home_is_set) { + // init_home(); + //} + break; + } +#endif // MAVLINK10 + + // Is this resolved? - MAVLink protocol change..... + case MAVLINK_MSG_ID_VFR_HUD: + { + // decode + mavlink_vfr_hud_t packet; + mavlink_msg_vfr_hud_decode(msg, &packet); + + // set airspeed + airspeed = 100*packet.airspeed; + break; + } +#ifdef MAVLINK10 + case MAVLINK_MSG_ID_HIL_STATE: + { + mavlink_hil_state_t packet; + mavlink_msg_hil_state_decode(msg, &packet); + + float vel = sqrt((packet.vx * packet.vx) + (packet.vy * packet.vy)); + float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100); + + // set gps hil sensor + g_gps->setHIL(packet.time_usec/1000.0, + packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, + vel*1.0e-2, cog*1.0e-2, 0, 0); + + #if HIL_MODE == HIL_MODE_SENSORS + + // rad/sec + Vector3f gyros; + gyros.x = (float)packet.xgyro / 1000.0; + gyros.y = (float)packet.ygyro / 1000.0; + gyros.z = (float)packet.zgyro / 1000.0; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; + + imu.set_gyro(gyros); + + imu.set_accel(accels); + + #else + + // set dcm hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + + #endif + + break; + } +#endif // MAVLINK10 +#endif +#if HIL_MODE == HIL_MODE_ATTITUDE + case MAVLINK_MSG_ID_ATTITUDE: + { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + // rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM + Vector3f gyros; + gyros.x = (float)packet.rollspeed / 1000.0; + gyros.y = (float)packet.pitchspeed / 1000.0; + gyros.z = (float)packet.yawspeed / 1000.0; + + imu.set_gyro(gyros); + + // m/s/s + Vector3f accels; + accels.x = (float)packet.roll * gravity / 1000.0; + accels.y = (float)packet.pitch * gravity / 1000.0; + accels.z = (float)packet.yaw * gravity / 1000.0; + + imu.set_accel(accels); + break; + } +#endif +#if HIL_MODE == HIL_MODE_SENSORS + + case MAVLINK_MSG_ID_RAW_IMU: + { + // decode + mavlink_raw_imu_t packet; + mavlink_msg_raw_imu_decode(msg, &packet); + + // set imu hil sensors + // TODO: check scaling for temp/absPress + float temp = 70; + float absPress = 1; + //Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc); + //Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro); + + // rad/sec + Vector3f gyros; + gyros.x = (float)packet.xgyro / 1000.0; + gyros.y = (float)packet.ygyro / 1000.0; + gyros.z = (float)packet.zgyro / 1000.0; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; + + imu.set_gyro(gyros); + + imu.set_accel(accels); + + compass.setHIL(packet.xmag,packet.ymag,packet.zmag); + break; + } + + case MAVLINK_MSG_ID_RAW_PRESSURE: + { + // decode + mavlink_raw_pressure_t packet; + mavlink_msg_raw_pressure_decode(msg, &packet); + + // set pressure hil sensor + // TODO: check scaling + float temp = 70; + barometer.setHIL(temp,packet.press_diff1 + 101325); + break; + } +#endif // HIL_MODE + +#if MOUNT == ENABLED + case MAVLINK_MSG_ID_MOUNT_CONFIGURE: + { + camera_mount.configure_msg(msg); + break; + } + + case MAVLINK_MSG_ID_MOUNT_CONTROL: + { + camera_mount.control_msg(msg); + break; + } + + case MAVLINK_MSG_ID_MOUNT_STATUS: + { + camera_mount.status_msg(msg); + break; + } +#endif // MOUNT == ENABLED + } // end switch +} // end handle mavlink + +uint16_t +GCS_MAVLINK::_count_parameters() +{ + // if we haven't cached the parameter count yet... + if (0 == _parameter_count) { + AP_Param *vp; + AP_Param::ParamToken token; + + vp = AP_Param::first(&token, NULL); + do { + _parameter_count++; + } while (NULL != (vp = AP_Param::next_scalar(&token, NULL))); + } + return _parameter_count; +} + +/** +* @brief Send the next pending parameter, called from deferred message +* handling code +*/ +void +GCS_MAVLINK::queued_param_send() +{ + // Check to see if we are sending parameters + if (NULL == _queued_parameter) return; + + AP_Param *vp; + float value; + + // copy the current parameter and prepare to move to the next + vp = _queued_parameter; + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(_queued_parameter_type); + + char param_name[ONBOARD_PARAM_NAME_LENGTH]; + vp->copy_name(param_name, sizeof(param_name)); + + mavlink_msg_param_value_send( + chan, + param_name, + value, + mav_var_type(_queued_parameter_type), + _queued_parameter_count, + _queued_parameter_index); + + _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type); + _queued_parameter_index++; +} + +/** +* @brief Send the next pending waypoint, called from deferred message +* handling code +*/ +void +GCS_MAVLINK::queued_waypoint_send() +{ + if (waypoint_receiving && + waypoint_request_i <= (unsigned)g.command_total) { + mavlink_msg_waypoint_request_send( + chan, + waypoint_dest_sysid, + waypoint_dest_compid, + waypoint_request_i); + } +} + +/* + a delay() callback that processes MAVLink packets. We set this as the + callback in long running library initialisation routines to allow + MAVLink to process packets while waiting for the initialisation to + complete +*/ +static void mavlink_delay(unsigned long t) +{ + unsigned long tstart; + static unsigned long last_1hz, last_50hz; + + if (in_mavlink_delay) { + // this should never happen, but let's not tempt fate by + // letting the stack grow too much + delay(t); + return; + } + + in_mavlink_delay = true; + + tstart = millis(); + do { + unsigned long tnow = millis(); + if (tnow - last_1hz > 1000) { + last_1hz = tnow; + gcs_send_message(MSG_HEARTBEAT); + gcs_send_message(MSG_EXTENDED_STATUS1); + } + if (tnow - last_50hz > 20) { + last_50hz = tnow; + gcs_update(); + } + delay(1); +#if USB_MUX_PIN > 0 + check_usb_mux(); +#endif + } while (millis() - tstart < t); + + in_mavlink_delay = false; +} + +/* + send a message on both GCS links + */ +static void gcs_send_message(enum ap_message id) +{ + gcs0.send_message(id); + if (gcs3.initialised) { + gcs3.send_message(id); + } +} + +/* + send data streams in the given rate range on both links + */ +static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) +{ + gcs0.data_stream_send(freqMin, freqMax); + if (gcs3.initialised) { + gcs3.data_stream_send(freqMin, freqMax); + } +} + +/* + look for incoming commands on the GCS links + */ +static void gcs_update(void) +{ + gcs0.update(); + if (gcs3.initialised) { + gcs3.update(); + } +} + +static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) +{ + gcs0.send_text(severity, str); + if (gcs3.initialised) { + gcs3.send_text(severity, str); + } +} + +/* + send a low priority formatted message to the GCS + only one fits in the queue, so if you send more than one before the + last one gets into the serial buffer then the old one will be lost + */ +static void gcs_send_text_fmt(const prog_char_t *fmt, ...) +{ + char fmtstr[40]; + va_list ap; + uint8_t i; + for (i=0; i" + " erase (all logs)\n" + " enable | all\n" + " disable | 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 diff --git a/APMrover2/Makefile b/APMrover2/Makefile new file mode 100644 index 0000000000..84b05b1e04 --- /dev/null +++ b/APMrover2/Makefile @@ -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) + diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h new file mode 100644 index 0000000000..8060df868a --- /dev/null +++ b/APMrover2/Parameters.h @@ -0,0 +1,510 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#include + +// Global parameter class. +// +class Parameters { +public: + // The 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")), + + ************************************************************/ + diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde new file mode 100644 index 0000000000..c52d3832a9 --- /dev/null +++ b/APMrover2/Parameters.pde @@ -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); + } +} diff --git a/APMrover2/command_description.txt b/APMrover2/command_description.txt new file mode 100644 index 0000000000..2d3102c9d9 --- /dev/null +++ b/APMrover2/command_description.txt @@ -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 + diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde new file mode 100644 index 0000000000..0d853e1e71 --- /dev/null +++ b/APMrover2/commands.pde @@ -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(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + target_bearing = get_bearing(¤t_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(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + target_bearing = get_bearing(¤t_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; + +} + diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde new file mode 100644 index 0000000000..c4241143e2 --- /dev/null +++ b/APMrover2/commands_logic.pde @@ -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(); +} + diff --git a/APMrover2/commands_process.pde b/APMrover2/commands_process.pde new file mode 100644 index 0000000000..b6e98f3076 --- /dev/null +++ b/APMrover2/commands_process.pde @@ -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; + } +} diff --git a/APMrover2/config.h b/APMrover2/config.h new file mode 100644 index 0000000000..85514f3258 --- /dev/null +++ b/APMrover2/config.h @@ -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 diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde new file mode 100644 index 0000000000..68a3e899fd --- /dev/null +++ b/APMrover2/control_modes.pde @@ -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 +} + + diff --git a/APMrover2/createTags b/APMrover2/createTags new file mode 100644 index 0000000000..1b9c3aff76 --- /dev/null +++ b/APMrover2/createTags @@ -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 :!ctags -R --sort=yes --c++-kinds=+p --fields=+iaS --extra=+q . + +#" 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 diff --git a/APMrover2/defines.h b/APMrover2/defines.h new file mode 100644 index 0000000000..fe0f4e93da --- /dev/null +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/events.pde b/APMrover2/events.pde new file mode 100644 index 0000000000..de38c848ca --- /dev/null +++ b/APMrover2/events.pde @@ -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(); + } + } +} diff --git a/APMrover2/failsafe.pde b/APMrover2/failsafe.pde new file mode 100644 index 0000000000..a1d2b80c7d --- /dev/null +++ b/APMrover2/failsafe.pde @@ -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)); + } + } +} diff --git a/APMrover2/geofence.pde b/APMrover2/geofence.pde new file mode 100644 index 0000000000..41223ae1e5 --- /dev/null +++ b/APMrover2/geofence.pde @@ -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; iboundary[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 diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde new file mode 100644 index 0000000000..f74396b318 --- /dev/null +++ b/APMrover2/navigation.pde @@ -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(¤t_loc, &next_WP); + + if (wp_distance < 0){ + gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); + //Serial.println(wp_distance,DEC); + return; + } + + // target_bearing is where we should be heading + // -------------------------------------------- + target_bearing = get_bearing(¤t_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() +{ + +} + diff --git a/APMrover2/planner.pde b/APMrover2/planner.pde new file mode 100644 index 0000000000..94ef061e46 --- /dev/null +++ b/APMrover2/planner.pde @@ -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; +} + diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde new file mode 100644 index 0000000000..23c972de73 --- /dev/null +++ b/APMrover2/radio.pde @@ -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(); +} diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde new file mode 100644 index 0000000000..2df661650b --- /dev/null +++ b/APMrover2/sensors.pde @@ -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 +} + diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde new file mode 100644 index 0000000000..6bd7099916 --- /dev/null +++ b/APMrover2/setup.pde @@ -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 diff --git a/APMrover2/system.pde b/APMrover2/system.pde new file mode 100644 index 0000000000..e5898eb034 --- /dev/null +++ b/APMrover2/system.pde @@ -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(" GROUND START")); + + #if(GROUND_START_DELAY > 0) + gcs_send_text_P(SEVERITY_LOW,PSTR(" 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(" zero airspeed calibrated")); + } else { + gcs_send_text_P(SEVERITY_LOW,PSTR(" 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 diff --git a/APMrover2/test.pde b/APMrover2/test.pde new file mode 100644 index 0000000000..2c1c051d85 --- /dev/null +++ b/APMrover2/test.pde @@ -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