diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h deleted file mode 100644 index e75fb175cf..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h +++ /dev/null @@ -1,925 +0,0 @@ -// -// 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_NONE No GPS attached -// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU. -// GPS_PROTOCOL_MTK MediaTek-based GPS. -// 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_UBLOX -// - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_SENSOR OPTIONAL -// AIRSPEED_RATIO OPTIONAL -// -// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached. -// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed -// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s -// -// The default assumes that an airspeed sensor is connected. -// -#define AIRSPEED_SENSOR ENABLED -#define AIRSPEED_RATIO 1.9936 -// -#define MAGNETOMETER 1 -#define USE_MAGNETOMETER ENABLED - -////////////////////////////////////////////////////////////////////////////// -// GCS_PROTOCOL OPTIONAL -// GCS_PORT OPTIONAL -// -// The GCS_PROTOCOL option determines which (if any) ground control station -// protocol will be used. Must be one of: -// -// GCS_PROTOCOL_NONE No GCS output -// GCS_PROTOCOL_STANDARD standard APM protocol -// GCS_PROTOCOL_SPECIAL special test protocol (?) -// GCS_PROTOCOL_LEGACY legacy ArduPilot protocol -// GCS_PROTOCOL_XPLANE HIL simulation ground station -// GCS_PROTOCOL_IMU ArdiPilot IMU output -// GCS_PROTOCOL_JASON Jason's special secret GCS protocol -// GCS_PROTOCOL_DEBUGTERMINAL In-flight debug console (experimental) -// -// The GCS_PORT option determines which serial port will be used by the -// GCS protocol. The usual values are 0 for the console/USB port, -// or 3 for the telemetry port on the oilpan. Note that some protocols -// will ignore this value and always use the console port. -// -// The default GCS protocol is the standard ArduPilot Mega protocol. -// -// The default serial port is the telemetry port for GCS_PROTOCOL_STANDARD -// and GCS_PROTOCOL_LEGACY. For all other protocols, the default serial -// port is the FTDI/console port. GCS_PORT normally should not be set -// in your configuration. -// -#define GCS_PROTOCOL GCS_PROTOCOL_XDIY -#define GCS_PORT 0 -// - -////////////////////////////////////////////////////////////////////////////// -// Serial port speeds. -// -// SERIAL0_BAUD OPTIONAL -// -// Baudrate for the console port. Default is 38400bps. -// -// SERIAL3_BAUD OPTIONAL -// -// Baudrate for the telemetry port. Default is 115200bps. -// -//#define SERIAL0_BAUD 38400 -//#define SERIAL3_BAUD 115200 -// - -////////////////////////////////////////////////////////////////////////////// -// Battery monitoring OPTIONAL -// -// See the manual for details on selecting divider resistors for battery -// monitoring via the oilpan. -// -// BATTERY_EVENT OPTIONAL -// -// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is -// DISABLED. -// -// BATTERY_TYPE OPTIONAL if BATTERY_EVENT is set -// -// Set to 0 for 3s LiPo, 1 for 4s LiPo. The default is 0, selecting a 3s -// battery. -// -// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set. -// -// Normally derived from BATTERY_TYPE, the automatic value can be overridden -// here. Value in volts at which ArduPilot Mega should consider the -// battery to be "low". -// -// VOLT_DIV_RATIO OPTIONAL -// -// See the manual for details. The default value corresponds to the resistors -// recommended by the manual. -// -//#define BATTERY_EVENT DISABLED -//#define BATTERY_TYPE 0 -//#define LOW_VOLTAGE 11.4 -//#define VOLT_DIV_RATIO 3.0 -// - -////////////////////////////////////////////////////////////////////////////// -// INPUT_VOLTAGE OPTIONAL -// -// In order to have accurate pressure and battery voltage readings, this -// value should be set to the voltage measured on the 5V rail on the oilpan. -// -// See the manual for more details. The default value should be close. -// -#define INPUT_VOLTAGE 5.0 -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// FLIGHT_MODE OPTIONAL -// FLIGHT_MODE_CHANNEL OPTIONAL -// -// Flight modes assigned to the control channel, and the input channel that -// is read for the control mode. -// -// Use a servo tester, or the ArduPilotMega_demo test program to check your -// switch settings. -// -// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and -// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option -// uses channel numbers 1-8 (and defaults to 8). -// -// If you only have a three-position switch or just want three modes, set your -// switch to produce 1165, 1425, and 1815 microseconds and configure -// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default. -// -// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control -// channel connected to input channel 8, the hardware failsafe mode will -// activate for any control input over 1750ms. -// -// For more modes (up to six), set your switch(es) to produce any of 1165, -// 1295, 1425, 1555, 1685, and 1815 microseconds. -// -// Flight mode | Switch Setting (ms) -// ------------+--------------------- -// 1 | 1165 -// 2 | 1295 -// 3 | 1425 -// 4 | 1555 -// 5 | 1685 -// 6 | 1815 (FAILSAFE if using channel 8) -// -// The following standard flight modes are available: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// MANUAL | Full manual control via the hardware multiplexer. -// | -// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs. -// | -// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle. -// | -// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle. -// | -// RTL | Returns to the Home location and then LOITERs at a safe altitude. -// | -// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter -// | application or your Ground Control System to edit and upload -// | waypoints and other commands. -// | -// -// -// The following non-standard modes are EXPERIMENTAL: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// LOITER | Flies in a circle around the current location. -// | -// CIRCLE | Flies in a stabilized 'dumb' circle. -// | -// -// -// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and -// FLIGHT_MODE_6 should be MANUAL. -// -// -#define FLIGHT_MODE_CHANNEL 8 -// -//#define FLIGHT_MODE_1 RTL -//#define FLIGHT_MODE_2 RTL -//#define FLIGHT_MODE_3 FLY_BY_WIRE_A -//#define FLIGHT_MODE_4 FLY_BY_WIRE_A -//#define FLIGHT_MODE_5 MANUAL -//#define FLIGHT_MODE_6 MANUAL - -// Define the pulse width when to switch to next higher FLIGHT_MODE -#define FLIGHT_MODE_1_BOUNDARY 1125 -#define FLIGHT_MODE_2_BOUNDARY 1335 -#define FLIGHT_MODE_3_BOUNDARY 1550 -#define FLIGHT_MODE_4_BOUNDARY 1690 -#define FLIGHT_MODE_5_BOUNDARY 1880 - - - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_FAILSAFE OPTIONAL -// THROTTLE_FS_VALUE OPTIONAL -// THROTTLE_FAILSAFE_ACTION OPTIONAL -// -// The throttle failsafe allows you to configure a software failsafe activated -// by a setting on the throttle input channel (channel 3). -// -// This can be used to achieve a failsafe override on loss of radio control -// without having to sacrifice one of your FLIGHT_MODE settings, as the -// throttle failsafe overrides the switch-selected mode. -// -// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default -// is for it to be disabled. -// -// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value -// below which the failsafe engages. The default is 975ms, which is a very low -// throttle setting. Most transmitters will let you trim the manual throttle -// position up so that you cannot engage the failsafe with a regular stick movement. -// -// Configure your receiver's failsafe setting for the throttle channel to the -// absolute minimum, and use the ArduPilotMega_demo program to check that -// you cannot reach that value with the throttle control. Leave a margin of -// at least 50 microseconds between the lowest throttle setting and -// THROTTLE_FS_VALUE. -// -// The FAILSAFE_ACTION setting determines what APM will do when throttle failsafe -// mode is entered while flying in AUTO mode. This is important in order to avoid -// accidental failsafe behaviour when flying waypoints that take the aircraft -// temporarily out of radio range. -// -// If FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes, -// the aircraft will head for home in RTL mode. If the throttle channel moves -// back up, it will return to AUTO or LOITER mode. -// -// The default behaviour is to ignore throttle failsafe in AUTO and LOITER modes. -// -//#define THROTTLE_FAILSAFE DISABLED -//#define THROTTLE_FS_VALUE 975 -//#define THROTTLE_FAILSAFE_ACTION 2 -// - -////////////////////////////////////////////////////////////////////////////// -// AUTO_TRIM OPTIONAL -// -// ArduPilot Mega can update its trim settings by looking at the -// radio inputs when switching out of MANUAL mode. This allows you to -// manually trim your aircraft before switching to an assisted mode, but it -// also means that you should avoid switching out of MANUAL while you have -// any control stick deflection. -// -// The default is to enable AUTO_TRIM. -// -#define AUTO_TRIM ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_REVERSE OPTIONAL -// -// A few speed controls require the throttle servo signal be reversed. Setting -// this to ENABLED will reverse the throttle output signal. Ensure that your -// throttle needs to be reversed by using the hardware failsafe and the -// ArduPilotMega_demo program before setting this option. -// -// The default is to not reverse the signal. -// -//#define THROTTLE_REVERSE DISABLED -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_STICK_MIXING OPTIONAL -// -// If this option is set to ENABLED, manual control inputs are are respected -// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.) -// -// The default is to enable stick mixing, allowing the pilot to take -// emergency action without switching modes. -// -#define ENABLE_STICK_MIXING ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_OUT DEBUG -// -// When debugging, it can be useful to disable the throttle output. Set -// this option to DISABLED to disable throttle output signals. -// -// The default is to not disable throttle output. -// -#define THROTTLE_OUT DISABLED -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// STARTUP BEHAVIOUR -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GROUND_START_DELAY OPTIONAL -// -// If configured, inserts a delay between power-up and the beginning of IMU -// calibration during a ground start. -// -// Use this setting to give you time to position the aircraft horizontally -// for the IMU calibration. -// -// The default is to begin IMU calibration immediately at startup. -// -//#define GROUND_START_DELAY 0 -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_AIR_START OPTIONAL -// -// If air start is disabled then you will get a ground start (including IMU -// calibration) every time the AP is powered up. This means that if you get -// a power glitch or reboot for some reason in the air, you will probably -// crash, but it prevents a lot of problems on the ground like unintentional -// motor start-ups, etc. -// -// If air start is enabled then you will get an air start at power up and a -// ground start will be performed if the speed is near zero when we get gps -// lock. -// -// The default is to disable air start. -// -#define ENABLE_AIR_START 0 -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// FLIGHT AND NAVIGATION CONTROL -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Altitude measurement and control. -// -// AOA OPTIONAL -// -// The angle in 100ths of a degree that the nose of the aircraft should be -// raised from horizontal in level flight. The default is 1 degree. -// -//#define AOA 100 // note, 100ths of a degree -// -// ALT_EST_GAIN OPTIONAL -// -// The gain of the altitude estimation function; a lower number results -// in slower error correction and smoother output. The default is a -// reasonable starting point. -// -//#define ALT_EST_GAIN 0.01 -// -// ALTITUDE_MIX OPTIONAL -// -// Configures the blend between GPS and pressure altitude. -// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc. -// -// The default is to use only GPS altitude. -// -//#define ALTITUDE_MIX 0 -// - - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_HIL OPTIONAL -// -// This will output a binary control string to for use in HIL sims -// such as Xplane 9 or FlightGear. -// -//#define ENABLE_HIL ENABLED -// - - -////////////////////////////////////////////////////////////////////////////// -// 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 15 -// - -////////////////////////////////////////////////////////////////////////////// -// FLY_BY_WIRE_B airspeed control -// -// 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. -// -// THROTTLE_ALT_P OPTIONAL -// THROTTLE_ALT_I OPTIONAL -// THROTTLE_ALT_D OPTIONAL -// -// P, I and D terms for the throttle control loop. Defaults are 0.5, 0, 0. -// -// THROTTLE_ALT_INT_MAX OPTIONAL -// -// Maximum throttle input due to the integral. Limits the throttle input -// due to persistent inability to maintain the commanded speed. Helps -// prevent the throttle from staying wide open when the control is reduced -// after a period at maxium speed. -// Default is 20 (20%). -// -//#define AIRSPEED_FBW_MIN 6 -//#define AIRSPEED_FBW_MAX 30 -//#define THROTTLE_ALT_P 0.32 -//#define THROTTLE_ALT_I 0.04 -//#define THROTTLE_ALT_D 0.0 -//#define THROTTLE_ALT_INT_MAX 20 -// - -////////////////////////////////////////////////////////////////////////////// -// Throttle control -// -// THROTTLE_MIN OPTIONAL -// -// The minimum throttle setting to which the autopilot will reduce the -// throttle while descending. The default is zero, which is -// suitable for aircraft with a steady power-off glide. Increase this -// value if your aircraft needs throttle to maintain a stable descent in -// level flight. -// -// THROTTLE_CRUISE OPTIONAL -// -// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight. -// The default is 45%, which is reasonable for a modestly powered aircraft. -// -// THROTTLE_MAX OPTIONAL -// -// The maximum throttle setting the autopilot will apply. The default is 75%. -// Reduce this value if your aicraft is overpowered, or has complex flight -// characteristics at high throttle settings. -// -//#define THROTTLE_MIN 0 -//#define THROTTLE_CRUISE 45 -//#define THROTTLE_MAX 75 -// - -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -// HEAD_MAX OPTIONAL -// -// The maximum commanded bank angle in either direction. -// The default is 45 degrees. Decrease this value if your aircraft is not -// stable or has difficulty maintaining altitude in a steep bank. -// -// PITCH_MAX OPTIONAL -// -// The maximum commanded pitch up angle. -// The default is 15 degrees. Care should be taken not to set this value too -// large, as the aircraft may stall. -// -// PITCH_MIN -// -// The maximum commanded pitch down angle. Note that this value must be -// negative. The default is -25 degrees. Care should be taken not to set -// this value too large as it may result in overspeeding the aircraft. -// -//#define HEAD_MAX 45 -//#define PITCH_MAX 15 -//#define PITCH_MIN -25 - -////////////////////////////////////////////////////////////////////////////// -// Attitude control gains -// -// Tuning values for the attitude control PID loops. -// -// The P term is the primary tuning value. This determines how the control -// deflection varies in proportion to the required correction. -// -// The I term is used to help control surfaces settle. This value should -// normally be kept low. -// -// The D term is used to control overshoot. Avoid using or adjusting this -// term if you are not familiar with tuning PID loops. It should normally -// be zero for most aircraft. -// -// Note: When tuning these values, start with changes of no more than 25% at -// a time. -// -// SERVO_ROLL_P OPTIONAL -// SERVO_ROLL_I OPTIONAL -// SERVO_ROLL_D OPTIONAL -// -// P, I and D terms for roll control. Defaults are 0.4, 0, 0. -// -// SERVO_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// ROLL_SLEW_LIMIT EXPERIMENTAL -// -// Limits the slew rate of the roll control in degrees per second. If zero, -// slew rate is not limited. Default is to not limit the roll control slew rate. -// (This feature is currently not implemented.) -// -// SERVO_PITCH_P OPTIONAL -// SERVO_PITCH_I OPTIONAL -// SERVO_PITCH_D OPTIONAL -// -// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0. -// -// SERVO_PITCH_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. native flight -// AoA). If you find this value is insufficient, consider adjusting the AOA -// parameter. -// Default is 5 degrees. -// -// PITCH_COMP OPTIONAL -// -// Adds pitch input to compensate for the loss of lift due to roll control. -// Default is 0.20 (20% of roll control also applied to pitch control). -// -// SERVO_YAW_P OPTIONAL -// SERVO_YAW_I OPTIONAL -// SERVO_YAW_D OPTIONAL -// -// P, I and D terms for the YAW control. Defaults are 0.5, 0, 0. -// -// SERVO_YAW_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// RUDDER_MIX OPTIONAL -// -// Roll to yaw mixing. This allows for co-ordinated turns. -// Default is 0.50 (50% of roll control also applied to yaw control.) -// -//#define SERVO_ROLL_P 0.4 -//#define SERVO_ROLL_I 0.0 -//#define SERVO_ROLL_D 0.0 -//#define SERVO_ROLL_INT_MAX 5 -//#define ROLL_SLEW_LIMIT 0 -//#define SERVO_PITCH_P 0.6 -//#define SERVO_PITCH_I 0.0 -//#define SERVO_PITCH_D 0.0 -//#define SERVO_PITCH_INT_MAX 5 -//#define PITCH_COMP 0.2 -//#define SERVO_YAW_P 0.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.01, 0.02. -// -// NAV_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// NAV_PITCH_ASP_P OPTIONAL -// NAV_PITCH_ASP_I OPTIONAL -// NAV_PITCH_ASP_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain airspeed. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ASP_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed airspeed). -// Default is 5 degrees. -// -// NAV_PITCH_ALT_P OPTIONAL -// NAV_PITCH_ALT_I OPTIONAL -// NAV_PITCH_ALT_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain altitude. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ALT_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed altitude). -// Default is 5 degrees. -// -//#define NAV_ROLL_P 0.7 -//#define NAV_ROLL_I 0.01 -//#define NAV_ROLL_D 0.02 -//#define NAV_ROLL_INT_MAX 5 -//#define NAV_PITCH_ASP_P 0.65 -//#define NAV_PITCH_ASP_I 0.0 -//#define NAV_PITCH_ASP_D 0.0 -//#define NAV_PITCH_ASP_INT_MAX 5 -//#define NAV_PITCH_ALT_P 0.65 -//#define NAV_PITCH_ALT_I 0.0 -//#define NAV_PITCH_ALT_D 0.0 -//#define NAV_PITCH_ALT_INT_MAX 5 -// - -////////////////////////////////////////////////////////////////////////////// -// Energy/Altitude control gains -// -// The Energy/altitude control system uses throttle input to control aircraft -// altitude. -// -// The P term is the primary tuning value. This determines how the throttle -// setting varies in proportion to the required correction. -// -// The I term is used to compensate for small offsets. -// -// The D term is used to control overshoot. Avoid adjusting this term if -// you are not familiar with tuning PID loops. -// -// THROTTLE_TE_P OPTIONAL -// THROTTLE_TE_I OPTIONAL -// THROTTLE_TE_D OPTIONAL -// -// P, I and D terms for throttle adjustments made to control altitude. -// Defaults are 0.5, 0, 0. -// -// THROTTLE_TE_INT_MAX OPTIONAL -// -// Maximum throttle input due to the integral term. This limits the -// throttle from being overdriven due to a persistent offset (e.g. -// inability to maintain the programmed altitude). -// Default is 20%. -// -// THROTTLE_SLEW_LIMIT OPTIONAL -// -// Limits the slew rate of the throttle, in percent per second. Helps -// avoid sudden throttle changes, which can destabilise the aircraft. -// A setting of zero disables the feature. -// Default is zero (disabled). -// -// P_TO_T OPTIONAL -// -// Pitch to throttle feed-forward gain. Used when AIRSPEED_SENSOR -// is DISABLED. Default is 2.5. -// -//#define THROTTLE_TE_P 0.50 -//#define THROTTLE_TE_I 0.0 -//#define THROTTLE_TE_D 0.0 -//#define THROTTLE_TE_INT_MAX 20 -//#define THROTTLE_SLEW_LIMIT 0 -//#define P_TO_T 2.5 -// - -////////////////////////////////////////////////////////////////////////////// -// Crosstrack compensation -// -// XTRACK_GAIN OPTIONAL -// -// Crosstrack compensation in degrees per metre off track. -// Default value is 0.01 degrees per metre. Values lower than 0.001 will -// disable crosstrack compensation. -// -// XTRACK_ENTRY_ANGLE OPTIONAL -// -// Maximum angle used to correct for track following. -// Default value is 30 degrees. -// -//#define XTRACK_GAIN 0.01 -//#define XTRACK_ENTRY_ANGLE 30 -// - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// DEBUGGING -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Subsystem test and debug. -// -// DEBUG_SUBSYSTEM DEBUG -// -// Selects a subsystem debug mode. Default is 0. -// -// 0 = no debug -// 1 = Debug the Radio input -// 2 = Radio Setup / Servo output -// 4 = Debug the GPS input -// 5 = Debug the GPS input - RAW OUTPUT -// 6 = Debug the IMU -// 7 = Debug the Control Switch -// 8 = Debug the Servo DIP switches -// 9 = Debug the Relay out -// 10 = Debug the Magnetometer -// 11 = Debug the ABS pressure sensor -// 12 = Debug the stored waypoints -// 13 = Debug the Throttle -// 14 = Debug the Radio Min Max -// 15 = Debug the EEPROM - Hex Dump -// 16 = XBee X-CTU Range and RSSI Test -// 17 = Debug IMU - raw gyro and accel outputs -// 20 = Debug Analog Sensors -// -// -//#define DEBUG_SUBSYSTEM 0 -// - -////////////////////////////////////////////////////////////////////////////// -// DEBUG_LEVEL DEBUG -// -// Selects the lowest level of debug messages passed to the telemetry system. -// Default is SEVERITY_LOW. May be one of: -// -// SEVERITY_LOW -// SEVERITY_MEDIUM -// SEVERITY_HIGH -// SEVERITY_CRITICAL -// -//#define DEBUG_LEVEL SEVERITY_LOW -// - -////////////////////////////////////////////////////////////////////////////// -// Dataflash logging control -// -// Each of these logging options may be set to ENABLED to enable, or DISABLED -// to disable the logging of the respective data. -// -// LOG_ATTITUDE_FAST DEBUG -// -// Logs basic attitude info to the dataflash at 50Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_ATTITUDE_MED OPTIONAL -// -// Logs basic attitude info to the dataflash at 10Hz (uses less space than -// LOG_ATTITUDE_FAST). Defaults to ENABLED. -// -// LOG_GPS OPTIONAL -// -// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED. -// -// LOG_PM OPTIONAL -// -// Logs IMU performance monitoring info every 20 seconds. -// Defaults to DISABLED. -// -// LOG_CTUN OPTIONAL -// -// Logs control loop tuning info at 10 Hz. This information is useful for tuning -// servo control loop gain values. Defaults to DISABLED. -// -// LOG_NTUN OPTIONAL -// -// Logs navigation tuning info at 10 Hz. This information is useful for tuning -// navigation control loop gain values. Defaults to DISABLED. -// -// LOG_ MODE OPTIONAL -// -// Logs changes to the flight mode upon occurrence. Defaults to ENABLED. -// -// LOG_RAW DEBUG -// -// Logs raw accelerometer and gyro data at 50 Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_CMD OPTIONAL -// -// Logs new commands when they process. -// Defaults to ENABLED. -// -//#define LOG_ATTITUDE_FAST DISABLED -//#define LOG_ATTITUDE_MED ENABLED -//#define LOG_GPS ENABLED -//#define LOG_PM ENABLED -//#define LOG_CTUN DISABLED -//#define LOG_NTUN DISABLED -//#define LOG_MODE ENABLED -//#define LOG_RAW DISABLED -//#define LOG_CMD ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// Navigation defaults -// -// WP_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the waypoint radius -// (the radius from a target waypoint within which the APM will consider -// itself to have arrived at the waypoint) to this value in meters. This is -// mainly intended to allow users to start using the APM without running the -// WaypointWriter first. -// -// LOITER_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the loiter radius -// (the distance the APM will attempt to maintain from a waypoint while -// loitering) to this value in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// -//#define WP_RADIUS_DEFAULT 20 -//#define LOITER_RADIUS_DEFAULT 30 -// - -////////////////////////////////////////////////////////////////////////////// -// 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. -// - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference deleted file mode 100644 index b05a3c2af5..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference +++ /dev/null @@ -1,914 +0,0 @@ -// -// 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_NONE No GPS attached -// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU. -// GPS_PROTOCOL_MTK MediaTek-based GPS. -// 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_UBLOX -// - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_SENSOR OPTIONAL -// AIRSPEED_RATIO OPTIONAL -// -// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached. -// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed -// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s -// -// The default assumes that an airspeed sensor is connected. -// -//#define AIRSPEED_SENSOR ENABLED -//#define AIRSPEED_RATIO 1.9936 -// - -////////////////////////////////////////////////////////////////////////////// -// GCS_PROTOCOL OPTIONAL -// GCS_PORT OPTIONAL -// -// The GCS_PROTOCOL option determines which (if any) ground control station -// protocol will be used. Must be one of: -// -// GCS_PROTOCOL_NONE No GCS output -// GCS_PROTOCOL_STANDARD standard APM protocol -// GCS_PROTOCOL_SPECIAL special test protocol (?) -// GCS_PROTOCOL_LEGACY legacy ArduPilot protocol -// GCS_PROTOCOL_XPLANE HIL simulation ground station -// GCS_PROTOCOL_IMU ArdiPilot IMU output -// GCS_PROTOCOL_JASON Jason's special secret GCS protocol -// GCS_PROTOCOL_DEBUGTERMINAL In-flight debug console (experimental) -// -// The GCS_PORT option determines which serial port will be used by the -// GCS protocol. The usual values are 0 for the console/USB port, -// or 3 for the telemetry port on the oilpan. Note that some protocols -// will ignore this value and always use the console port. -// -// The default GCS protocol is the standard ArduPilot Mega protocol. -// -// The default serial port is the telemetry port for GCS_PROTOCOL_STANDARD -// and GCS_PROTOCOL_LEGACY. For all other protocols, the default serial -// port is the FTDI/console port. GCS_PORT normally should not be set -// in your configuration. -// -//#define GCS_PROTOCOL GCS_PROTOCOL_STANDARD -//#define GCS_PORT 3 -// - -////////////////////////////////////////////////////////////////////////////// -// Serial port speeds. -// -// SERIAL0_BAUD OPTIONAL -// -// Baudrate for the console port. Default is 38400bps. -// -// SERIAL3_BAUD OPTIONAL -// -// Baudrate for the telemetry port. Default is 115200bps. -// -//#define SERIAL0_BAUD 38400 -//#define SERIAL3_BAUD 115200 -// - -////////////////////////////////////////////////////////////////////////////// -// Battery monitoring OPTIONAL -// -// See the manual for details on selecting divider resistors for battery -// monitoring via the oilpan. -// -// BATTERY_EVENT OPTIONAL -// -// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is -// DISABLED. -// -// BATTERY_TYPE OPTIONAL if BATTERY_EVENT is set -// -// Set to 0 for 3s LiPo, 1 for 4s LiPo. The default is 0, selecting a 3s -// battery. -// -// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set. -// -// Normally derived from BATTERY_TYPE, the automatic value can be overridden -// here. Value in volts at which ArduPilot Mega should consider the -// battery to be "low". -// -// VOLT_DIV_RATIO OPTIONAL -// -// See the manual for details. The default value corresponds to the resistors -// recommended by the manual. -// -//#define BATTERY_EVENT DISABLED -//#define BATTERY_TYPE 0 -//#define LOW_VOLTAGE 11.4 -//#define VOLT_DIV_RATIO 3.0 -// - -////////////////////////////////////////////////////////////////////////////// -// INPUT_VOLTAGE OPTIONAL -// -// In order to have accurate pressure and battery voltage readings, this -// value should be set to the voltage measured on the 5V rail on the oilpan. -// -// See the manual for more details. The default value should be close. -// -//#define INPUT_VOLTAGE 5.0 -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// FLIGHT_MODE OPTIONAL -// FLIGHT_MODE_CHANNEL OPTIONAL -// -// Flight modes assigned to the control channel, and the input channel that -// is read for the control mode. -// -// Use a servo tester, or the ArduPilotMega_demo test program to check your -// switch settings. -// -// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and -// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option -// uses channel numbers 1-8 (and defaults to 8). -// -// If you only have a three-position switch or just want three modes, set your -// switch to produce 1165, 1425, and 1815 microseconds and configure -// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default. -// -// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control -// channel connected to input channel 8, the hardware failsafe mode will -// activate for any control input over 1750ms. -// -// For more modes (up to six), set your switch(es) to produce any of 1165, -// 1295, 1425, 1555, 1685, and 1815 microseconds. -// -// Flight mode | Switch Setting (ms) -// ------------+--------------------- -// 1 | 1165 -// 2 | 1295 -// 3 | 1425 -// 4 | 1555 -// 5 | 1685 -// 6 | 1815 (FAILSAFE if using channel 8) -// -// The following standard flight modes are available: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// MANUAL | Full manual control via the hardware multiplexer. -// | -// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs. -// | -// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle. -// | -// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle. -// | -// RTL | Returns to the Home location and then LOITERs at a safe altitude. -// | -// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter -// | application or your Ground Control System to edit and upload -// | waypoints and other commands. -// | -// -// -// The following non-standard modes are EXPERIMENTAL: -// -// Name | Description -// -----------------+-------------------------------------------- -// | -// LOITER | Flies in a circle around the current location. -// | -// CIRCLE | Flies in a stabilized 'dumb' circle. -// | -// -// -// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and -// FLIGHT_MODE_6 should be MANUAL. -// -// -//#define FLIGHT_MODE_CHANNEL 8 -// -//#define FLIGHT_MODE_1 RTL -//#define FLIGHT_MODE_2 RTL -//#define FLIGHT_MODE_3 FLY_BY_WIRE_A -//#define FLIGHT_MODE_4 FLY_BY_WIRE_A -//#define FLIGHT_MODE_5 MANUAL -//#define FLIGHT_MODE_6 MANUAL -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_FAILSAFE OPTIONAL -// THROTTLE_FS_VALUE OPTIONAL -// THROTTLE_FAILSAFE_ACTION OPTIONAL -// -// The throttle failsafe allows you to configure a software failsafe activated -// by a setting on the throttle input channel (channel 3). -// -// This can be used to achieve a failsafe override on loss of radio control -// without having to sacrifice one of your FLIGHT_MODE settings, as the -// throttle failsafe overrides the switch-selected mode. -// -// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default -// is for it to be disabled. -// -// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value -// below which the failsafe engages. The default is 975ms, which is a very low -// throttle setting. Most transmitters will let you trim the manual throttle -// position up so that you cannot engage the failsafe with a regular stick movement. -// -// Configure your receiver's failsafe setting for the throttle channel to the -// absolute minimum, and use the ArduPilotMega_demo program to check that -// you cannot reach that value with the throttle control. Leave a margin of -// at least 50 microseconds between the lowest throttle setting and -// THROTTLE_FS_VALUE. -// -// The FAILSAFE_ACTION setting determines what APM will do when throttle failsafe -// mode is entered while flying in AUTO mode. This is important in order to avoid -// accidental failsafe behaviour when flying waypoints that take the aircraft -// temporarily out of radio range. -// -// If FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes, -// the aircraft will head for home in RTL mode. If the throttle channel moves -// back up, it will return to AUTO or LOITER mode. -// -// The default behaviour is to ignore throttle failsafe in AUTO and LOITER modes. -// -//#define THROTTLE_FAILSAFE DISABLED -//#define THROTTLE_FS_VALUE 975 -//#define THROTTLE_FAILSAFE_ACTION 2 -// - -////////////////////////////////////////////////////////////////////////////// -// AUTO_TRIM OPTIONAL -// -// ArduPilot Mega can update its trim settings by looking at the -// radio inputs when switching out of MANUAL mode. This allows you to -// manually trim your aircraft before switching to an assisted mode, but it -// also means that you should avoid switching out of MANUAL while you have -// any control stick deflection. -// -// The default is to enable AUTO_TRIM. -// -//#define AUTO_TRIM ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_REVERSE OPTIONAL -// -// A few speed controls require the throttle servo signal be reversed. Setting -// this to ENABLED will reverse the throttle output signal. Ensure that your -// throttle needs to be reversed by using the hardware failsafe and the -// ArduPilotMega_demo program before setting this option. -// -// The default is to not reverse the signal. -// -//#define THROTTLE_REVERSE DISABLED -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_STICK_MIXING OPTIONAL -// -// If this option is set to ENABLED, manual control inputs are are respected -// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.) -// -// The default is to enable stick mixing, allowing the pilot to take -// emergency action without switching modes. -// -//#define ENABLE_STICK_MIXING ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_OUT DEBUG -// -// When debugging, it can be useful to disable the throttle output. Set -// this option to DISABLED to disable throttle output signals. -// -// The default is to not disable throttle output. -// -//#define THROTTLE_OUT ENABLED -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// STARTUP BEHAVIOUR -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GROUND_START_DELAY OPTIONAL -// -// If configured, inserts a delay between power-up and the beginning of IMU -// calibration during a ground start. -// -// Use this setting to give you time to position the aircraft horizontally -// for the IMU calibration. -// -// The default is to begin IMU calibration immediately at startup. -// -//#define GROUND_START_DELAY 0 -// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_AIR_START OPTIONAL -// -// If air start is disabled then you will get a ground start (including IMU -// calibration) every time the AP is powered up. This means that if you get -// a power glitch or reboot for some reason in the air, you will probably -// crash, but it prevents a lot of problems on the ground like unintentional -// motor start-ups, etc. -// -// If air start is enabled then you will get an air start at power up and a -// ground start will be performed if the speed is near zero when we get gps -// lock. -// -// The default is to disable air start. -// -//#define ENABLE_AIR_START 0 -// - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// FLIGHT AND NAVIGATION CONTROL -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Altitude measurement and control. -// -// AOA OPTIONAL -// -// The angle in 100ths of a degree that the nose of the aircraft should be -// raised from horizontal in level flight. The default is 1 degree. -// -//#define AOA 100 // note, 100ths of a degree -// -// ALT_EST_GAIN OPTIONAL -// -// The gain of the altitude estimation function; a lower number results -// in slower error correction and smoother output. The default is a -// reasonable starting point. -// -//#define ALT_EST_GAIN 0.01 -// -// ALTITUDE_MIX OPTIONAL -// -// Configures the blend between GPS and pressure altitude. -// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc. -// -// The default is to use only GPS altitude. -// -//#define ALTITUDE_MIX 0 -// - - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_HIL OPTIONAL -// -// This will output a binary control string to for use in HIL sims -// such as Xplane 9 or FlightGear. -// -//#define ENABLE_HIL ENABLED -// - - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_CRUISE OPTIONAL -// -// The speed in metres per second to maintain during cruise. The default -// is 10m/s, which is a conservative value suitable for relatively small, -// light aircraft. -// -//#define AIRSPEED_CRUISE 10 -// - -////////////////////////////////////////////////////////////////////////////// -// FLY_BY_WIRE_B airspeed control -// -// 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. -// -// THROTTLE_ALT_P OPTIONAL -// THROTTLE_ALT_I OPTIONAL -// THROTTLE_ALT_D OPTIONAL -// -// P, I and D terms for the throttle control loop. Defaults are 0.5, 0, 0. -// -// THROTTLE_ALT_INT_MAX OPTIONAL -// -// Maximum throttle input due to the integral. Limits the throttle input -// due to persistent inability to maintain the commanded speed. Helps -// prevent the throttle from staying wide open when the control is reduced -// after a period at maxium speed. -// Default is 20 (20%). -// -//#define AIRSPEED_FBW_MIN 6 -//#define AIRSPEED_FBW_MAX 30 -//#define THROTTLE_ALT_P 0.32 -//#define THROTTLE_ALT_I 0.04 -//#define THROTTLE_ALT_D 0.0 -//#define THROTTLE_ALT_INT_MAX 20 -// - -////////////////////////////////////////////////////////////////////////////// -// Throttle control -// -// THROTTLE_MIN OPTIONAL -// -// The minimum throttle setting to which the autopilot will reduce the -// throttle while descending. The default is zero, which is -// suitable for aircraft with a steady power-off glide. Increase this -// value if your aircraft needs throttle to maintain a stable descent in -// level flight. -// -// THROTTLE_CRUISE OPTIONAL -// -// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight. -// The default is 45%, which is reasonable for a modestly powered aircraft. -// -// THROTTLE_MAX OPTIONAL -// -// The maximum throttle setting the autopilot will apply. The default is 75%. -// Reduce this value if your aicraft is overpowered, or has complex flight -// characteristics at high throttle settings. -// -//#define THROTTLE_MIN 0 -//#define THROTTLE_CRUISE 45 -//#define THROTTLE_MAX 75 -// - -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -// HEAD_MAX OPTIONAL -// -// The maximum commanded bank angle in either direction. -// The default is 45 degrees. Decrease this value if your aircraft is not -// stable or has difficulty maintaining altitude in a steep bank. -// -// PITCH_MAX OPTIONAL -// -// The maximum commanded pitch up angle. -// The default is 15 degrees. Care should be taken not to set this value too -// large, as the aircraft may stall. -// -// PITCH_MIN -// -// The maximum commanded pitch down angle. Note that this value must be -// negative. The default is -25 degrees. Care should be taken not to set -// this value too large as it may result in overspeeding the aircraft. -// -//#define HEAD_MAX 45 -//#define PITCH_MAX 15 -//#define PITCH_MIN -25 - -////////////////////////////////////////////////////////////////////////////// -// Attitude control gains -// -// Tuning values for the attitude control PID loops. -// -// The P term is the primary tuning value. This determines how the control -// deflection varies in proportion to the required correction. -// -// The I term is used to help control surfaces settle. This value should -// normally be kept low. -// -// The D term is used to control overshoot. Avoid using or adjusting this -// term if you are not familiar with tuning PID loops. It should normally -// be zero for most aircraft. -// -// Note: When tuning these values, start with changes of no more than 25% at -// a time. -// -// SERVO_ROLL_P OPTIONAL -// SERVO_ROLL_I OPTIONAL -// SERVO_ROLL_D OPTIONAL -// -// P, I and D terms for roll control. Defaults are 0.4, 0, 0. -// -// SERVO_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// ROLL_SLEW_LIMIT EXPERIMENTAL -// -// Limits the slew rate of the roll control in degrees per second. If zero, -// slew rate is not limited. Default is to not limit the roll control slew rate. -// (This feature is currently not implemented.) -// -// SERVO_PITCH_P OPTIONAL -// SERVO_PITCH_I OPTIONAL -// SERVO_PITCH_D OPTIONAL -// -// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0. -// -// SERVO_PITCH_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. native flight -// AoA). If you find this value is insufficient, consider adjusting the AOA -// parameter. -// Default is 5 degrees. -// -// PITCH_COMP OPTIONAL -// -// Adds pitch input to compensate for the loss of lift due to roll control. -// Default is 0.20 (20% of roll control also applied to pitch control). -// -// SERVO_YAW_P OPTIONAL -// SERVO_YAW_I OPTIONAL -// SERVO_YAW_D OPTIONAL -// -// P, I and D terms for the YAW control. Defaults are 0.5, 0, 0. -// -// SERVO_YAW_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// RUDDER_MIX OPTIONAL -// -// Roll to yaw mixing. This allows for co-ordinated turns. -// Default is 0.50 (50% of roll control also applied to yaw control.) -// -//#define SERVO_ROLL_P 0.4 -//#define SERVO_ROLL_I 0.0 -//#define SERVO_ROLL_D 0.0 -//#define SERVO_ROLL_INT_MAX 5 -//#define ROLL_SLEW_LIMIT 0 -//#define SERVO_PITCH_P 0.6 -//#define SERVO_PITCH_I 0.0 -//#define SERVO_PITCH_D 0.0 -//#define SERVO_PITCH_INT_MAX 5 -//#define PITCH_COMP 0.2 -//#define SERVO_YAW_P 0.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.01, 0.02. -// -// NAV_ROLL_INT_MAX OPTIONAL -// -// Maximum control offset due to the integral. This prevents the control -// output from being overdriven due to a persistent offset (e.g. crosstracking). -// Default is 5 degrees. -// -// NAV_PITCH_ASP_P OPTIONAL -// NAV_PITCH_ASP_I OPTIONAL -// NAV_PITCH_ASP_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain airspeed. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ASP_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed airspeed). -// Default is 5 degrees. -// -// NAV_PITCH_ALT_P OPTIONAL -// NAV_PITCH_ALT_I OPTIONAL -// NAV_PITCH_ALT_D OPTIONAL -// -// P, I and D terms for pitch adjustments made to maintain altitude. -// Defaults are 0.65, 0, 0. -// -// NAV_PITCH_ALT_INT_MAX OPTIONAL -// -// Maximum pitch offset due to the integral. This limits the control -// output from being overdriven due to a persistent offset (eg. inability -// to maintain the programmed altitude). -// Default is 5 degrees. -// -//#define NAV_ROLL_P 0.7 -//#define NAV_ROLL_I 0.01 -//#define NAV_ROLL_D 0.02 -//#define NAV_ROLL_INT_MAX 5 -//#define NAV_PITCH_ASP_P 0.65 -//#define NAV_PITCH_ASP_I 0.0 -//#define NAV_PITCH_ASP_D 0.0 -//#define NAV_PITCH_ASP_INT_MAX 5 -//#define NAV_PITCH_ALT_P 0.65 -//#define NAV_PITCH_ALT_I 0.0 -//#define NAV_PITCH_ALT_D 0.0 -//#define NAV_PITCH_ALT_INT_MAX 5 -// - -////////////////////////////////////////////////////////////////////////////// -// Energy/Altitude control gains -// -// The Energy/altitude control system uses throttle input to control aircraft -// altitude. -// -// The P term is the primary tuning value. This determines how the throttle -// setting varies in proportion to the required correction. -// -// The I term is used to compensate for small offsets. -// -// The D term is used to control overshoot. Avoid adjusting this term if -// you are not familiar with tuning PID loops. -// -// THROTTLE_TE_P OPTIONAL -// THROTTLE_TE_I OPTIONAL -// THROTTLE_TE_D OPTIONAL -// -// P, I and D terms for throttle adjustments made to control altitude. -// Defaults are 0.5, 0, 0. -// -// THROTTLE_TE_INT_MAX OPTIONAL -// -// Maximum throttle input due to the integral term. This limits the -// throttle from being overdriven due to a persistent offset (e.g. -// inability to maintain the programmed altitude). -// Default is 20%. -// -// THROTTLE_SLEW_LIMIT OPTIONAL -// -// Limits the slew rate of the throttle, in percent per second. Helps -// avoid sudden throttle changes, which can destabilise the aircraft. -// A setting of zero disables the feature. -// Default is zero (disabled). -// -// P_TO_T OPTIONAL -// -// Pitch to throttle feed-forward gain. Used when AIRSPEED_SENSOR -// is DISABLED. Default is 2.5. -// -//#define THROTTLE_TE_P 0.50 -//#define THROTTLE_TE_I 0.0 -//#define THROTTLE_TE_D 0.0 -//#define THROTTLE_TE_INT_MAX 20 -//#define THROTTLE_SLEW_LIMIT 0 -//#define P_TO_T 2.5 -// - -////////////////////////////////////////////////////////////////////////////// -// Crosstrack compensation -// -// XTRACK_GAIN OPTIONAL -// -// Crosstrack compensation in degrees per metre off track. -// Default value is 0.01 degrees per metre. Values lower than 0.001 will -// disable crosstrack compensation. -// -// XTRACK_ENTRY_ANGLE OPTIONAL -// -// Maximum angle used to correct for track following. -// Default value is 30 degrees. -// -//#define XTRACK_GAIN 0.01 -//#define XTRACK_ENTRY_ANGLE 30 -// - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// DEBUGGING -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Subsystem test and debug. -// -// DEBUG_SUBSYSTEM DEBUG -// -// Selects a subsystem debug mode. Default is 0. -// -// 0 = no debug -// 1 = Debug the Radio input -// 2 = Radio Setup / Servo output -// 4 = Debug the GPS input -// 5 = Debug the GPS input - RAW OUTPUT -// 6 = Debug the IMU -// 7 = Debug the Control Switch -// 8 = Debug the Servo DIP switches -// 9 = Debug the Relay out -// 10 = Debug the Magnetometer -// 11 = Debug the ABS pressure sensor -// 12 = Debug the stored waypoints -// 13 = Debug the Throttle -// 14 = Debug the Radio Min Max -// 15 = Debug the EEPROM - Hex Dump -// 16 = XBee X-CTU Range and RSSI Test -// 17 = Debug IMU - raw gyro and accel outputs -// 20 = Debug Analog Sensors -// -// -//#define DEBUG_SUBSYSTEM 0 -// - -////////////////////////////////////////////////////////////////////////////// -// DEBUG_LEVEL DEBUG -// -// Selects the lowest level of debug messages passed to the telemetry system. -// Default is SEVERITY_LOW. May be one of: -// -// SEVERITY_LOW -// SEVERITY_MEDIUM -// SEVERITY_HIGH -// SEVERITY_CRITICAL -// -//#define DEBUG_LEVEL SEVERITY_LOW -// - -////////////////////////////////////////////////////////////////////////////// -// Dataflash logging control -// -// Each of these logging options may be set to ENABLED to enable, or DISABLED -// to disable the logging of the respective data. -// -// LOG_ATTITUDE_FAST DEBUG -// -// Logs basic attitude info to the dataflash at 50Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_ATTITUDE_MED OPTIONAL -// -// Logs basic attitude info to the dataflash at 10Hz (uses less space than -// LOG_ATTITUDE_FAST). Defaults to ENABLED. -// -// LOG_GPS OPTIONAL -// -// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED. -// -// LOG_PM OPTIONAL -// -// Logs IMU performance monitoring info every 20 seconds. -// Defaults to DISABLED. -// -// LOG_CTUN OPTIONAL -// -// Logs control loop tuning info at 10 Hz. This information is useful for tuning -// servo control loop gain values. Defaults to DISABLED. -// -// LOG_NTUN OPTIONAL -// -// Logs navigation tuning info at 10 Hz. This information is useful for tuning -// navigation control loop gain values. Defaults to DISABLED. -// -// LOG_ MODE OPTIONAL -// -// Logs changes to the flight mode upon occurrence. Defaults to ENABLED. -// -// LOG_RAW DEBUG -// -// Logs raw accelerometer and gyro data at 50 Hz (uses more space). -// Defaults to DISABLED. -// -// LOG_CMD OPTIONAL -// -// Logs new commands when they process. -// Defaults to ENABLED. -// -//#define LOG_ATTITUDE_FAST DISABLED -//#define LOG_ATTITUDE_MED ENABLED -//#define LOG_GPS ENABLED -//#define LOG_PM ENABLED -//#define LOG_CTUN DISABLED -//#define LOG_NTUN DISABLED -//#define LOG_MODE ENABLED -//#define LOG_RAW DISABLED -//#define LOG_CMD ENABLED -// - -////////////////////////////////////////////////////////////////////////////// -// Navigation defaults -// -// WP_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the waypoint radius -// (the radius from a target waypoint within which the APM will consider -// itself to have arrived at the waypoint) to this value in meters. This is -// mainly intended to allow users to start using the APM without running the -// WaypointWriter first. -// -// LOITER_RADIUS_DEFAULT OPTIONAL -// -// When the user performs a factory reset on the APM, set the loiter radius -// (the distance the APM will attempt to maintain from a waypoint while -// loitering) to this value in meters. This is mainly intended to allow -// users to start using the APM without running the WaypointWriter first. -// -//#define WP_RADIUS_DEFAULT 20 -//#define LOITER_RADIUS_DEFAULT 30 -// - -////////////////////////////////////////////////////////////////////////////// -// 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/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h deleted file mode 100644 index 06a3078582..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h +++ /dev/null @@ -1,19 +0,0 @@ -#define DEBUG_SUBSYSTEM 0 - -#define FLIGHT_MODE_CHANNEL 8 -#define FLIGHT_MODE_1 AUTO -#define FLIGHT_MODE_2 RTL -#define FLIGHT_MODE_3 FLY_BY_WIRE_A -#define FLIGHT_MODE_4 FLY_BY_WIRE_B -#define FLIGHT_MODE_5 STABILIZE -#define FLIGHT_MODE_6 MANUAL - -#define GCS_PROTOCOL GCS_PROTOCOL_DEBUGTERMINAL -#define ENABLE_HIL ENABLED -#define GCS_PORT 3 -#define GPS_PROTOCOL GPS_PROTOCOL_IMU -#define AIRSPEED_CRUISE 25 - -#define THROTTLE_FAILSAFE ENABLED -#define AIRSPEED_SENSOR ENABLED - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde deleted file mode 100644 index f5dfaef424..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde +++ /dev/null @@ -1,845 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/* -ArduPilotMega Version 1.0.3 Public Alpha -Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short -Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi -Please contribute your ideas! - - -This firmware is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. -*/ - -// Libraries -#include -#include -#include -#include // ArduPilot Mega RC Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include // ArduPilot GPS library -#include -#include // ArduPilot Mega BMP085 Library -#include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Magnetometer Library - -// AVR runtime -#include -#include -#include -#include - -// Configuration -#include "config.h" - -// Local modules -#include "defines.h" - -// Serial ports -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port (except for GPS_PROTOCOL_IMU) -//FastSerialPort3(Serial3); // Telemetry port (optional, Standard and ArduPilot protocols only) - -// GPS selection -#if GPS_PROTOCOL == GPS_PROTOCOL_NMEA -AP_GPS_NMEA GPS(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF -AP_GPS_SIRF GPS(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX -AP_GPS_UBLOX GPS(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_IMU -AP_GPS_IMU GPS(&Serial); // note, console port -#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK -AP_GPS_MTK GPS(&Serial1); -#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE -AP_GPS_NONE GPS(NULL); -#else -# error Must define GPS_PROTOCOL in your configuration file. -#endif - -// The X-Plane GCS requires the IMU GPS configuration -#if (ENABLE_HIL == ENABLED) && (GPS_PROTOCOL != GPS_PROTOCOL_IMU) -# error Must select GPS_PROTOCOL_IMU when configuring for X-Plane -#endif - -// GENERAL VARIABLE DECLARATIONS -// -------------------------------------------- -byte control_mode = MANUAL; -boolean failsafe = false; // did our throttle dip below the failsafe value? -boolean ch3_failsafe = false; -byte crash_timer; -byte oldSwitchPosition; // for remembering the control mode switch -boolean reverse_switch = 1; // do we read the reversing switches after startup? - -byte ground_start_count = 6; // have we achieved first lock and set Home? -int ground_start_avg; // 5 samples to avg speed for ground start -boolean ground_start = false; // have we started on the ground? -char *comma = ","; - -char* flight_mode_strings[] = { - "Manual", - "Circle", - "Stabilize", - "", - "", - "FBW_A", - "FBW_B", - "", - "", - "", - "Auto", - "RTL", - "Loiter", - "Takeoff", - "Land"}; - - -/* Radio values - Channel assignments - 1 Ailerons (rudder if no ailerons) - 2 Elevator - 3 Throttle - 4 Rudder (if we have ailerons) - 5 Mode - 6 TBD - 7 TBD - 8 TBD -*/ - -int radio_min[8]; // may be reset by init sequence -int radio_trim[8]; // may be reset by init sequence -int radio_max[8]; // may be reset by init sequence -int radio_in[8]; // current values from the transmitter - microseconds -int radio_out[8]; // Send to the PWM library -int servo_out[8]; // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 - -int elevon1_trim = 1500; -int elevon2_trim = 1500; -int ch1_temp = 1500; // Used for elevon mixing -int ch2_temp = 1500; - -int reverse_roll = 1; -int reverse_pitch = 1; -int reverse_rudder = 1; -byte mix_mode = 0; // 0 = normal , 1 = elevons -int reverse_elevons = 1; -int reverse_ch1_elevon = 1; -int reverse_ch2_elevon = 1; -byte yaw_mode; - -byte flight_mode_channel; -byte flight_modes[6]; -byte auto_trim; - -// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon - - -/* PID Control variables - Cases - 1 Aileron servo control (rudder if no ailerons) - 2 Elevator servo control - 3 Rudder servo control (if we have ailerons) - 4 Roll set-point control - 5 Pitch set-point based on airspeed error control - 6 Pitch set-point based on altitude error control (if we do not have airspeed sensor) - 7 Throttle based on Energy Height (Total Energy) error control - 8 Throttle based on altitude error control -*/ - -float kp[8]; -float ki[8]; -float kd[8]; -uint16_t integrator_max[8]; // PID Integrator Max deg * 100 -float integrator[8]; // PID Integrators deg * 100 -long last_error[8]; // PID last error for derivative -float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind - - -/* Feed Forward gains - Cases - 1 Bank angle to desired pitch (Pitch Comp) - 2 Aileron Servo to Rudder Servo - 3 Pitch to Throttle -*/ - -float kff[3]; - - -// GPS variables -// ------------- -const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage -float scaleLongUp; // used to reverse longtitude scaling -float scaleLongDown; // used to reverse longtitude scaling -boolean GPS_light = false; // status of the GPS light - -// Location & Navigation -// --------------------- -byte wp_radius = 20; // meters -long hold_course = -1; // deg * 100 dir of plane -long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate -long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target -int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate -byte loiter_radius; // meters -float x_track_gain; -int x_track_angle; - -long head_max; -long pitch_max; -long pitch_min; -float altitude_mix; - -byte command_must_index; // current command memory location -byte command_may_index; // current command memory location -byte command_must_ID; // current command ID -byte command_may_ID; // current command ID -//byte EEPROM_command // 1 = from the list, 0 = generated - - -// Airspeed -// -------- -int airspeed; // m/s * 100 -int airspeed_cruise; // m/s * 100 : target airspeed sensor value -float airspeed_ratio; // used to scale airspeed -byte airspeed_fbw_min; // m/s -byte airspeed_fbw_max; // m/s - -// Throttle Failsafe -// ------------------ -byte throttle_failsafe_enabled; -int throttle_failsafe_value; -byte throttle_failsafe_action; -uint16_t log_bitmask; - -// Location Errors -// --------------- -long bearing_error; // deg * 100 : 0 to 36000 -long altitude_error; // meters * 100 we are off in altitude -float airspeed_error; // m/s * 100 -float crosstrack_error; // meters we are off trackline -long energy_error; // energy state error (kinetic + potential) for altitude hold -long airspeed_energy_error; // kinetic portion of energy error - -// Sensors -// -------- -float airpressure_raw; // Airspeed Sensor - is a float to better handle filtering -int airpressure_offset; // analog air pressure sensor while still -int airpressure; // airspeed as a pressure value -float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2, initialized above threshold for filter -float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3, initialized above threshold for filter -float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1+2+3+4, initialized above threshold for filter - -// From IMU -long roll_sensor; // degrees * 100 -long pitch_sensor; // degrees * 100 -long yaw_sensor; // degrees * 100 - -float roll; // radians -float pitch; // radians -float yaw; // radians - - -// Magnetometer variables -int magnetom_x; // Doug to do -int magnetom_y; -int magnetom_z; -float MAG_Heading; - -// Pressure Sensor variables -unsigned long abs_press; -unsigned long abs_press_filt; -unsigned long abs_press_gnd; -int ground_temperature; -int temp_unfilt; -long ground_alt; // Ground altitude from gps at startup in centimeters -long press_alt; // Pressure altitude - -// flight mode specific -// -------------------- -//boolean takeoff_complete = false; // Flag for using take-off controls -boolean land_complete = false; -int landing_pitch; // pitch for landing set by commands -int takeoff_pitch; -int takeoff_altitude; -int landing_distance; // meters; - -// Loiter management -// ----------------- -long old_target_bearing; // deg * 100 -int loiter_total; // deg : how many times to loiter * 360 -int loiter_delta; // deg : how far we just turned -int loiter_sum; // deg : how far we have turned around a waypoint -long loiter_time; // millis : when we started LOITER mode -int loiter_time_max; // millis : how long to stay in LOITER mode - -// these are the values for navigation control functions -// ---------------------------------------------------- -long nav_roll; // deg * 100 : target roll angle -long nav_pitch; // deg * 100 : target pitch angle -long altitude_estimate; // for smoothing GPS output - -int throttle_min; // 0-100 : target throttle output for average speed -int throttle_cruise; // 0-100 : target throttle output for average speed -int throttle_max; // 0-100 : target throttle output for average speed - -// Waypoints -// --------- -long wp_distance; // meters - distance between plane and next waypoint -long wp_totalDistance; // meters - distance between old and next waypoint -byte wp_total; // # of Commands total including way -byte wp_index; // Current active command index -byte next_wp_index; // Current active command index - -// repeating event control -// ----------------------- -byte event_id; // what to do - see defines -long event_timer; // when the event was asked for in ms -int event_delay; // how long to delay the next firing of event in millis -int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice -int event_value; // per command value, such as PWM for servos -int event_undo_value; // the value used to undo commands -byte repeat_forever; -byte undo_event; // counter for timing the undo - -// delay command -// -------------- -int delay_timeout = 0; // used to delay commands -long delay_start = 0; // used to delay commands - -// 3D Location vectors -// ------------------- -struct Location home; // home location -struct Location prev_WP; // last waypoint -struct Location current_loc; // current location -struct Location next_WP; // next waypoint -struct Location tell_command; // command for telemetry -struct Location next_command; // command preloaded -long target_altitude; // used for -long offset_altitude; // used for -int hold_alt_above_home; -boolean home_is_set = false; // Flag for if we have gps lock and have set the home location - - -// IMU variables -// ------------- - -//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ -uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware -int SENSOR_SIGN[] = { 1, -1, -1, - -1, 1, 1, - -1, -1, -1}; - -// Temp compensation curve constants -// These must be produced by measuring data and curve fitting -// [X / Y/Z gyro][A / B/C or 0 order / 1st order / 2nd order constants] -// values may migrate to a Config file -float GTC[3][3]={{1665, 0, 0}, - {1665, 0, 0}, - {1665, 0, 0}}; - -float AN[6]; // array that store the 6 ADC channels used by IMU -float AN_OFFSET[6]; // Array that store the Offset of the gyros and accelerometers -float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) -float Accel_Vector[3]; // Store the acceleration in a vector -float Accel_Vector_unfiltered[3]; // Store the acceleration in a vector -float Gyro_Vector[3]; // Store the gyros turn rate in a vector -float Omega_Vector[3]; // Corrected Gyro_Vector data -float Omega_P[3]; // Omega Proportional correction -float Omega_I[3]; // Omega Integrator -float Omega[3]; -float errorRollPitch[3]; -float errorYaw[3]; -float errorCourse; -float COGX; // Course overground X axis -float COGY = 1; // Course overground Y axis - -float DCM_Matrix[3][3] = {{1,0,0}, - {0,1,0}, - {0,0,1}}; - -float Update_Matrix[3][3] = {{0,1,2}, - {3,4,5}, - {6,7,8}}; - -float Temporary_Matrix[3][3]; - -// Performance monitoring -// ---------------------- -long perf_mon_timer; -float imu_health; // Metric based on accel gain deweighting -int G_Dt_max; // Max main loop cycle time in milliseconds -byte gyro_sat_count; -byte adc_constraints; -byte renorm_sqrt_count; -byte renorm_blowup_count; -int gps_fix_count; -byte gcs_messages_sent; - - -// GCS -// --- -char GCS_buffer[53]; - -// System Timers -// -------------- -unsigned long fast_loopTimer; // Time in miliseconds of main control loop -unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop -byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops -byte slow_loopCounter = 0; -byte superslow_loopCounter = 0; -unsigned long deltaMiliSeconds; // Delta Time in miliseconds -unsigned long dTnav; // Delta Time in milliseconds for navigation computations -int mainLoop_count; -unsigned long elapsedTime; // for doing custom events -unsigned int GPS_timer; - -//******************************************************************************************************************************** -//******************************************************************************************************************************** -//******************************************************************************************************************************** -//******************************************************************************************************************************** -//******************************************************************************************************************************** - -// Basic Initialization -//--------------------- -void setup() { - jeti_init(); - jeti_status(" ** X-DIY **"); - jeti_update; - init_ardupilot(); -} - -void loop() -{ - // We want this to execute at 50Hz if possible - // ------------------------------------------- - if (millis()-fast_loopTimer > 19) { - deltaMiliSeconds = millis() - fast_loopTimer; - G_Dt = (float)deltaMiliSeconds / 1000.f; - fast_loopTimer = millis(); - mainLoop_count++; - - // Execute the fast loop - // --------------------- - fast_loop(); - - // Execute the medium loop - // ----------------------- - medium_loop(); - - if (millis()- perf_mon_timer > 20000) { - if (mainLoop_count != 0) { - send_message(MSG_PERF_REPORT); - if (log_bitmask & MASK_LOG_PM) - Log_Write_Performance(); - - resetPerfData(); - } - } - } -} - -void fast_loop() -{ - // This is the fast loop - we want it to execute at 50Hz if possible - // ----------------------------------------------------------------- - if (deltaMiliSeconds > G_Dt_max) - G_Dt_max = deltaMiliSeconds; - - // Read radio - // ---------- - read_radio(); - - // check for throttle failsafe condition - // ------------------------------------ -#if THROTTLE_FAILSAFE == 1 - set_failsafe(ch3_failsafe); -#endif - - - // read in the plane's attitude - // ---------------------------- -#if GPS_PROTOCOL == GPS_PROTOCOL_IMU - GPS.update(); - airspeed = (float)GPS.airspeed; //airspeed is * 100 coming in from Xplane for accuracy - calc_airspeed_errors(); - - if(digitalRead(SLIDE_SWITCH_PIN) == 0) { - read_AHRS(); - roll_sensor = -roll_sensor; - pitch_sensor = -pitch_sensor; - //yaw_sensor = -yaw_sensor; - }else{ - roll_sensor = GPS.roll_sensor; - pitch_sensor = GPS.pitch_sensor; - yaw_sensor = GPS.ground_course; - } - -#else - // Read Airspeed - // ------------- - #if AIRSPEED_SENSOR == 1 - read_airspeed(); - #endif - - read_AHRS(); - - if (log_bitmask & MASK_LOG_ATTITUDE_FAST) - Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (uint16_t)yaw_sensor); - - if (log_bitmask & MASK_LOG_RAW) - Log_Write_Raw(); -#endif - - // altitude smoothing - // ------------------ - if (control_mode != FLY_BY_WIRE_B) - calc_altitude_error(); - - // custom code/exceptions for flight modes - // --------------------------------------- - update_current_flight_mode(); - - // apply desired roll, pitch and yaw to the plane - // ---------------------------------------------- - if (control_mode > MANUAL) - stabilize(); - - // write out the servo PWM values - // ------------------------------ - set_servos_4(); - -#if ENABLE_HIL - output_HIL(); -#endif - -#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL - readgcsinput(); -#endif -} - -void medium_loop() -{ - // This is the start of the medium (10 Hz) loop pieces - // ----------------------------------------- - switch(medium_loopCounter) { - - // This case deals with the GPS - //------------------------------- - case 0: - medium_loopCounter++; - update_GPS(); - - #if MAGNETOMETER == 1 - APM_Compass.Read(); // Read magnetometer - APM_Compass.Calculate(roll,pitch); // Calculate heading - #endif - - break; - - // This case performs some navigation computations - //------------------------------------------------ - case 1: - medium_loopCounter++; - - if(GPS.new_data){ - dTnav = millis() - medium_loopTimer; - medium_loopTimer = millis(); - } - - // calculate the plane's desired bearing - // ------------------------------------- - navigate(); - break; - - // unused? no, jeti gets updated :-) - //------------------------------ - case 2: - medium_loopCounter++; - - // perform next command - // -------------------- - update_commands(); - jeti_update(); - break; - - // This case deals with sending high rate telemetry - //------------------------------------------------- - case 3: - medium_loopCounter++; - - if ((log_bitmask & MASK_LOG_ATTITUDE_MED) && !(log_bitmask & MASK_LOG_ATTITUDE_FAST)) - Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (uint16_t)yaw_sensor); - - if (log_bitmask & MASK_LOG_CTUN) - Log_Write_Control_Tuning(); - - if (log_bitmask & MASK_LOG_NTUN) - Log_Write_Nav_Tuning(); - - if (log_bitmask & MASK_LOG_GPS) - Log_Write_GPS(GPS.time, current_loc.lat, current_loc.lng, GPS.altitude, current_loc.alt, (long) GPS.ground_speed, GPS.ground_course, GPS.fix, GPS.num_sats); - - send_message(MSG_ATTITUDE); // Sends attitude data - break; - - // This case controls the slow loop - //--------------------------------- - case 4: - medium_loopCounter=0; - slow_loop(); - break; - } -} - -void slow_loop() -{ - // This is the slow (3 1/3 Hz) loop pieces - //---------------------------------------- - switch (slow_loopCounter){ - case 0: - slow_loopCounter++; - superslow_loopCounter++; - if(superslow_loopCounter >=15) { - // keep track of what page is in use in the log - // *** We need to come up with a better scheme to handle this... - eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage()); - superslow_loopCounter = 0; - } - break; - - case 1: - slow_loopCounter++; - - // Read 3-position switch on radio - // ------------------------------- - read_control_switch(); - - // Read Control Surfaces/Mix switches - // ---------------------------------- - if(reverse_switch){ - update_servo_switches(); - } - - // Read main battery voltage if hooked up - does not read the 5v from radio - // ------------------------------------------------------------------------ - #if BATTERY_EVENT == 1 - read_battery(); - #endif - - // Read Baro pressure - // ------------------ - read_airpressure(); - break; - - case 2: - slow_loopCounter = 0; - update_events(); - break; - } -} - - -void update_GPS(void) -{ - GPS.update(); - update_GPS_light(); - - if (GPS.new_data && GPS.fix) { - GPS_timer = millis(); - send_message(MSG_LOCATION); - - // for performance - // --------------- - gps_fix_count++; - - if(ground_start_count > 1){ - ground_start_count--; - ground_start_avg += GPS.ground_speed; - - } else if (ground_start_count == 1) { - // We countdown N number of good GPS fixes - // so that the altitude is more accurate - // ------------------------------------- - if (current_loc.lat == 0) { - SendDebugln("!! bad loc"); - ground_start_count = 5; - - } else { - if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT) { - startup_ground(); - - if (log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - - init_home(); - } else if (ENABLE_AIR_START == 0) { - init_home(); - } - - ground_start_count = 0; - } - } - - - current_loc.lng = GPS.longitude; // Lon * 10**7 - current_loc.lat = GPS.latitude; // Lat * 10**7 - -#if GPS_PROTOCOL == GPS_PROTOCOL_IMU - current_loc.alt = GPS.altitude; -#else - current_loc.alt = ((1 - altitude_mix) * GPS.altitude) + (altitude_mix * press_alt); // alt_MSL centimeters (meters * 100) -#endif - - COGX = cos(ToRad(GPS.ground_course/100.0)); - COGY = sin(ToRad(GPS.ground_course/100.0)); - } -} - -void update_current_flight_mode(void) -{ - if(control_mode == AUTO){ - crash_checker(); - - switch(command_must_ID){ - case CMD_TAKEOFF: - #if USE_MAGNETOMETER == ENABLED - calc_nav_roll(); - #else - nav_roll = 0; - #endif - float scaler; - #if AIRSPEED_SENSOR == 1 - scaler = (float)airspeed / (float)airspeed_cruise; - nav_pitch = scaler * takeoff_pitch * 50; - #else - scaler = (float)GPS.ground_speed / (float)airspeed_cruise; - nav_pitch = scaler * takeoff_pitch * 50; - #endif - - nav_pitch = constrain(nav_pitch, 0l, (long)takeoff_pitch); - - servo_out[CH_THROTTLE] = throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle - // ****************************** - - // override pitch_sensor to limit porpoising - // pitch_sensor = constrain(pitch_sensor, -6000, takeoff_pitch * 100); - // throttle = passthrough - break; - - case CMD_LAND: - calc_nav_roll(); - - #if AIRSPEED_SENSOR == 1 - calc_nav_pitch(); - calc_throttle(); - #else - calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle - calc_throttle(); // throttle basedtest landing_pitch; // pitch held constant - #endif - - if (land_complete) { - servo_out[CH_THROTTLE] = 0; - } - break; - - default: - hold_course = -1; - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - } - }else{ - switch(control_mode){ - case RTL: - case LOITER: - hold_course = -1; - crash_checker(); - calc_nav_roll(); - calc_nav_pitch(); - calc_throttle(); - break; - - case FLY_BY_WIRE_A: - // fake Navigation output using sticks - nav_roll = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * head_max * reverse_roll) / 350; - nav_pitch = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 3500l * reverse_pitch) / 350; - - nav_roll = constrain(nav_roll, -head_max, head_max); - nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority - break; - - case FLY_BY_WIRE_B: - // fake Navigation output using sticks - // We use pitch_min because its magnitude is normally greater than pitch_max - nav_roll = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * head_max * reverse_roll) / 350; - altitude_error = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * pitch_min * -reverse_pitch) / 350; - nav_roll = constrain(nav_roll, -head_max, head_max); - - #if AIRSPEED_SENSOR == 1 - //airspeed_error = ((AIRSPEED_FBW_MAX - AIRSPEED_FBW_MIN) * servo_out[CH_THROTTLE] / 100) + AIRSPEED_FBW_MIN; - airspeed_error = ((int)(airspeed_fbw_max - airspeed_fbw_min) * servo_out[CH_THROTTLE]) + ((int)airspeed_fbw_min * 100); - // Intermediate calculation - airspeed_error is just desired airspeed at this point - airspeed_energy_error = (long)(((long)airspeed_error * (long)airspeed_error) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation - airspeed_error = (airspeed_error - airspeed); - - #endif - - calc_throttle(); - calc_nav_pitch(); - break; - - case STABILIZE: - nav_roll = 0; - nav_pitch = 0; - // throttle is passthrough - break; - - case CIRCLE: - // we have no GPS installed and have lost radio contact - // or we just want to fly around in a gentle circle w/o GPS - // ---------------------------------------------------- - nav_roll = head_max / 3; - nav_pitch = 0; - - if (failsafe == true){ - servo_out[CH_THROTTLE] = throttle_cruise; - } - break; - - case MANUAL: - // servo_out is for Sim control only - // --------------------------------- - servo_out[CH_ROLL] = reverse_roll * (radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 9; - servo_out[CH_PITCH] = reverse_pitch * (radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 9; - servo_out[CH_RUDDER] = reverse_rudder * (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 9; - break; - //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - - } - } -} - -void read_AHRS(void) -{ - // Get gyro and accel data and perform IMU calculations - //----------------------------------------------------- - Read_adc_raw(); // Get current values for IMU sensors - Matrix_update(); // Integrate the DCM matrix - Normalize(); // Normalize the DCM matrix - Drift_correction(); // Perform drift correction - Euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation -} - - - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde deleted file mode 100644 index 97850fa696..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde +++ /dev/null @@ -1,343 +0,0 @@ -//**************************************************************** -// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. -//**************************************************************** - -void stabilize() -{ - float ch1_inf = 1.0; - float ch2_inf = 1.0; - float ch4_inf = 1.0; - - if(crash_timer > 0){ - nav_roll = 0; - } - - // For Testing Only - // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; - // Serial.print(" roll_sensor "); - // Serial.print(roll_sensor,DEC); - - // Calculate dersired servo output for the roll - // --------------------------------------------- - servo_out[CH_ROLL] = PID((nav_roll - roll_sensor), deltaMiliSeconds, CASE_SERVO_ROLL, 1.0); - servo_out[CH_PITCH] = PID((nav_pitch + abs(roll_sensor * kff[CASE_PITCH_COMP]) - pitch_sensor), deltaMiliSeconds, CASE_SERVO_PITCH, 1.0); - //Serial.print(" servo_out[CH_ROLL] "); - //Serial.print(servo_out[CH_ROLL],DEC); - - // Mix Stick input to allow users to override control surfaces - // ----------------------------------------------------------- - if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { - - ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim[CH_ROLL]; - ch1_inf = abs(ch1_inf); - ch1_inf = min(ch1_inf, 400.0); - ch1_inf = ((400.0 - ch1_inf) /400.0); - - ch2_inf = (float)radio_in[CH_PITCH] - radio_trim[CH_PITCH]; - ch2_inf = abs(ch2_inf); - ch2_inf = min(ch2_inf, 400.0); - ch2_inf = ((400.0 - ch2_inf) /400.0); - - // scale the sensor input based on the stick input - // ----------------------------------------------- - servo_out[CH_ROLL] *= ch1_inf; - servo_out[CH_PITCH] *= ch2_inf; - - // Mix in stick inputs - // ------------------- - servo_out[CH_ROLL] += reverse_roll * (radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 9; - servo_out[CH_PITCH] += reverse_pitch * (radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 9; - - //Serial.print(" servo_out[CH_ROLL] "); - //Serial.println(servo_out[CH_ROLL],DEC); - } - - // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes - // important for steering on the ground during landing - // ----------------------------------------------- - if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { - ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim[CH_RUDDER]; - ch4_inf = abs(ch4_inf); - ch4_inf = min(ch4_inf, 400.0); - ch4_inf = ((400.0 - ch4_inf) /400.0); - } - - // Apply output to Rudder - // ---------------------- - calc_nav_yaw(); - servo_out[CH_RUDDER] *= ch4_inf; - servo_out[CH_RUDDER] += reverse_rudder * (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 9; - - // Call slew rate limiter if used - // ------------------------------ - //#if(ROLL_SLEW_LIMIT != 0) - // servo_out[CH_ROLL] = roll_slew_limit(servo_out[CH_ROLL]); - //#endif -} - -void crash_checker() -{ - if(pitch_sensor < -4500){ - crash_timer = 255; - } - if(crash_timer > 0) - crash_timer--; -} - - -#if AIRSPEED_SENSOR == 0 -void calc_throttle() -{ - // no airspeed sensor, we use nav pitch to determin the proper throttle output - // AUTO, RTL, etc - // --------------------------------------------------------------------------- - if (nav_pitch >= 0) { - servo_out[CH_THROTTLE] = throttle_cruise + (THROTTLE_MAX - throttle_cruise) * nav_pitch / pitch_max; - } else { - servo_out[CH_THROTTLE] = throttle_cruise - (throttle_cruise - THROTTLE_MIN) * nav_pitch / pitch_min; - } - servo_out[CH_THROTTLE] = max(servo_out[CH_THROTTLE], 0); - - // are we going too slow? up the throttle to get some more groundspeed - // move into PID loop in the future - // lower the contstant value to limit the effect : 50 = default - int gs_boost = 30 * (1.0 - ((float)GPS.ground_speed / (float)airspeed_cruise)); - gs_boost = max(0, gs_boost); - servo_out[CH_THROTTLE] += gs_boost; - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], THROTTLE_MIN, THROTTLE_MAX); -} -#endif - -#if AIRSPEED_SENSOR == 1 -void calc_throttle() -{ - // throttle control with airspeed compensation - // ------------------------------------------- - energy_error = airspeed_energy_error + (float)altitude_error * 0.098f; - - // positive energy errors make the throttle go higher - servo_out[CH_THROTTLE] = throttle_cruise + PID(energy_error, dTnav, CASE_TE_THROTTLE, 1.0); - servo_out[CH_THROTTLE] = max(servo_out[CH_THROTTLE], 0); - - // are we going too slow? up the throttle to get some more groundspeed - // move into PID loop in the future - // lower the contstant value to limit the effect : 50 = default - - //JASON - We really should change this to act on airspeed in this case. Let's visit about it.... - /*int gs_boost = 30 * (1.0 - ((float)GPS.ground_speed / (float)airspeed_cruise)); - gs_boost = max(0,gs_boost); - servo_out[CH_THROTTLE] += gs_boost;*/ - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], THROTTLE_MIN, THROTTLE_MAX); -} -#endif - - - -/***************************************** - * Calculate desired roll angle (in medium freq loop) - *****************************************/ -// Yaw is separated into a function for future implementation of heading hold on take-off -// ---------------------------------------------------------------------------------------- -void calc_nav_yaw() -{ - // read_adc(4) is y axis accel; - // Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero) - servo_out[CH_RUDDER] = kff[CASE_RUDDER_MIX] * servo_out[CH_ROLL] + PID((long)read_adc(4), deltaMiliSeconds, CASE_SERVO_RUDDER, 1.0); - -} - - -#if AIRSPEED_SENSOR == 1 -void calc_nav_pitch() -{ - // Calculate the Pitch of the plane - // -------------------------------- - nav_pitch = -PID(airspeed_error, dTnav, CASE_NAV_PITCH_ASP, 1.0); - nav_pitch = constrain(nav_pitch, pitch_min, pitch_max); -} -#endif - -#if AIRSPEED_SENSOR == 0 -void calc_nav_pitch() -{ - // Calculate the Pitch of the plane - // -------------------------------- - nav_pitch = PID(altitude_error, dTnav, CASE_NAV_PITCH_ALT, 1.0); - nav_pitch = constrain(nav_pitch, pitch_min, pitch_max); -} -#endif - - -void calc_nav_roll() -{ - - // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. - // This does not make provisions for wind speed in excess of airframe speed - nav_gain_scaler = (float)GPS.ground_speed / (float)(AIRSPEED_CRUISE * 100); - nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4); - - // Jason -> I will implement the servo gain scaler some time, but it is independant of this scaling. - // Doug to implement the high speed servo gain scale - // use head max to limit turns, make a var - - // negative error = left turn - // positive error = right turn - // Calculate the required roll of the plane - // ---------------------------------------- - nav_roll = PID(bearing_error, dTnav, CASE_NAV_ROLL, nav_gain_scaler); //returns desired bank angle in degrees*100 - nav_roll = constrain(nav_roll,-head_max, head_max); - -} - - -/***************************************** - * Roll servo slew limit - *****************************************/ -/* -float roll_slew_limit(float servo) -{ - static float last; - float temp = constrain(servo, last-ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f); - last = servo; - return temp; -}*/ - -/***************************************** - * Throttle slew limit - *****************************************/ -/*float throttle_slew_limit(float throttle) -{ - static float last; - float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f); - last = throttle; - return temp; -} -*/ - -/***************************************** - * Proportional Integrator Derivative Control - *****************************************/ - -float PID(long PID_error, long dt, int PID_case, float scaler) -{ - // PID_case is used to keep track of which PID controller is being used - e.g. PID_servo_out[CH_ROLL] - float output, derivative; - - derivative = 1000.0f * (float)(PID_error - last_error[PID_case]) / (float)dt; - last_error[PID_case] = PID_error; - output = (kp[PID_case] * scaler * (float)PID_error); // Compute proportional component - //Positive error produces positive output - - integrator[PID_case] += (float)PID_error * ki[PID_case] * scaler * (float)dt / 1000.0f; - integrator[PID_case] = constrain(integrator[PID_case], -1.0*(float)integrator_max[PID_case], (float)integrator_max[PID_case]); - - output += integrator[PID_case]; // Add the integral component - output += kd[PID_case] * scaler * derivative; // Add the derivative component - return output; -} - -// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. -// Keeps outdated data out of our calculations -void reset_I(void) -{ - integrator[CASE_NAV_ROLL] = 0; - integrator[CASE_NAV_PITCH_ASP] = 0; - integrator[CASE_NAV_PITCH_ALT] = 0; - integrator[CASE_TE_THROTTLE] = 0; - integrator[CASE_ALT_THROTTLE] = 0; - - last_error[CASE_NAV_ROLL] = 0; - last_error[CASE_NAV_PITCH_ASP] = 0; - last_error[CASE_NAV_PITCH_ALT] = 0; - last_error[CASE_TE_THROTTLE] = 0; - last_error[CASE_ALT_THROTTLE] = 0; -} - -/***************************************** -* Set the flight control servos based on the current calculated values -*****************************************/ -void set_servos_4(void) -{ - if(control_mode == MANUAL){ - // do a direct pass through of radio values - for(int y=0; y<4; y++) { - radio_out[y] = radio_in[y]; - } - - } else { - if (mix_mode == 0){ - radio_out[CH_ROLL] = radio_trim[CH_ROLL] + (reverse_roll * ((float)servo_out[CH_ROLL] * 0.11111)); - radio_out[CH_PITCH] = radio_trim[CH_PITCH] + (reverse_pitch * ((float)servo_out[CH_PITCH] * 0.11111)); - radio_out[CH_RUDDER] = radio_trim[CH_RUDDER] + (reverse_rudder * ((float)servo_out[CH_RUDDER] * 0.11111)); - }else{ - /*Elevon mode*/ - float ch1; - float ch2; - ch1 = reverse_elevons * (servo_out[CH_PITCH] - servo_out[CH_ROLL]); - ch2 = servo_out[CH_PITCH] + servo_out[CH_ROLL]; - radio_out[CH_ROLL] = elevon1_trim + (reverse_ch1_elevon * (ch1 * 0.11111)); - radio_out[CH_PITCH] = elevon2_trim + (reverse_ch2_elevon * (ch2 * 0.11111)); - } - - #if THROTTLE_OUT == 0 - radio_out[CH_THROTTLE] = 0; - #endif - - - // convert 0 to 100% into PWM - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); - radio_out[CH_THROTTLE] = servo_out[CH_THROTTLE] * ((radio_max[CH_THROTTLE] - radio_min[CH_THROTTLE]) / 100); - radio_out[CH_THROTTLE] += radio_min[CH_THROTTLE]; - - //Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938 - - #if THROTTLE_REVERSE == 1 - radio_out[CH_THROTTLE] = radio_max[CH_THROTTLE] + radio_min[CH_THROTTLE] - radio_out[CH_THROTTLE]; - #endif - } - - // send values to the PWM timers for output - // ---------------------------------------- - for(int y=0; y<4; y++) { - //radio_out[y] = constrain(radio_out[y], radio_min[y], radio_max[y]); - APM_RC.OutputCh(y, radio_out[y]); // send to Servos - //Serial.print(radio_out[y],DEC); - //Serial.print(", "); - } - //Serial.println(" "); - } - -void demo_servos(byte i) { - - while(i > 0){ - send_message(SEVERITY_LOW,"Demo Servos!"); - APM_RC.OutputCh(1, 1400); - delay(400); - APM_RC.OutputCh(1, 1600); - delay(200); - APM_RC.OutputCh(1, 1500); - delay(400); - i--; - } -} - -int readOutputCh(unsigned char ch) -{ - int pwm; - switch(ch) - { - case 0: pwm = OCR5B; break; // ch0 - case 1: pwm = OCR5C; break; // ch1 - case 2: pwm = OCR1B; break; // ch2 - case 3: pwm = OCR1C; break; // ch3 - case 4: pwm = OCR4C; break; // ch4 - case 5: pwm = OCR4B; break; // ch5 - case 6: pwm = OCR3C; break; // ch6 - case 7: pwm = OCR3B; break; // ch7 - case 8: pwm = OCR5A; break; // ch8, PL3 - case 9: pwm = OCR1A; break; // ch9, PB5 - case 10: pwm = OCR3A; break; // ch10, PE3 - } - pwm >>= 1; // pwm / 2; - return pwm; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde deleted file mode 100644 index 07e0bb8df9..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde +++ /dev/null @@ -1,387 +0,0 @@ - -// Read the 6 ADC channels needed for the IMU -// ----------------- -void Read_adc_raw(void) -{ - int tc_temp = APM_ADC.Ch(GYRO_TEMP_CH); - for (int i = 0; i < 6; i++) { - AN[i] = APM_ADC.Ch(sensors[i]); - if (i < 3) { - AN[i] -= gyro_temp_comp(i, tc_temp); // Subtract temp compensated typical gyro bias - } else { - AN[i] -= 2025; // Subtract typical accel bias - } - } -} - -// Returns the temperature compensated raw gyro value -//--------------------------------------------------- -float gyro_temp_comp(int i, int temp) -{ - // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2 - //------------------------------------------------------------------------ - return GTC[i][0] + GTC[i][1] * temp + GTC[i][2] * temp * temp; -} - -// Returns an analog value with the offset removed -// ----------------- -float read_adc(int select) -{ - float temp; - if (SENSOR_SIGN[select] < 0) - temp = (AN_OFFSET[select]-AN[select]); - else - temp = (AN[select]-AN_OFFSET[select]); - - if (abs(temp) > ADC_CONSTRAINT) - adc_constraints++; // We keep track of the number of times we constrain the ADC output for performance reporting - -/* -// For checking the pitch/roll drift correction gain time constants -switch (select) { - case 3: - return 0; - break; - case 4: - return 0; - break; - case 5: - return 400; - break; -} -*/ - - -//End of drift correction gain test code - - return constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values -} - -/**************************************************/ -void Normalize(void) -{ - float error = 0; - float temporary[3][3]; - float renorm = 0; - boolean problem = FALSE; - - error= -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; // eq.19 - - Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); // eq.19 - Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); // eq.19 - - Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19 - Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19 - - Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b // eq.20 - - renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]); - if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion - renorm = .5 * (3 - renorm); // eq.21 - } else if (renorm < 100.0f && renorm > 0.01f) { - renorm = 1. / sqrt(renorm); - renorm_sqrt_count++; - } else { - problem = TRUE; - renorm_blowup_count++; - } - Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); - - renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]); - if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion - renorm = .5 * (3 - renorm); // eq.21 - } else if (renorm < 100.0f && renorm > 0.01f) { - renorm = 1. / sqrt(renorm); - renorm_sqrt_count++; - } else { - problem = TRUE; - renorm_blowup_count++; - } - Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); - - renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]); - if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion - renorm = .5 * (3 - renorm); // eq.21 - } else if (renorm < 100.0f && renorm > 0.01f) { - renorm = 1. / sqrt(renorm); - renorm_sqrt_count++; - } else { - problem = TRUE; - renorm_blowup_count++; - } - Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); - - if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! - DCM_Matrix[0][0] = 1.0f; - DCM_Matrix[0][1] = 0.0f; - DCM_Matrix[0][2] = 0.0f; - DCM_Matrix[1][0] = 0.0f; - DCM_Matrix[1][1] = 1.0f; - DCM_Matrix[1][2] = 0.0f; - DCM_Matrix[2][0] = 0.0f; - DCM_Matrix[2][1] = 0.0f; - DCM_Matrix[2][2] = 1.0f; - problem = FALSE; - } -} - -/**************************************************/ -void Drift_correction(void) -{ - //Compensation the Roll, Pitch and Yaw drift. - float mag_heading_x; - float mag_heading_y; - float errorCourse = 0; - static float Scaled_Omega_P[3]; - static float Scaled_Omega_I[3]; - float Accel_magnitude; - float Accel_weight; - float Integrator_magnitude; - - //*****Roll and Pitch*************** - - // Calculate the magnitude of the accelerometer vector - Accel_magnitude = sqrt(Accel_Vector[0] * Accel_Vector[0] + Accel_Vector[1] * Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]); - Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. - - // Dynamic weighting of accelerometer info (reliability filter) - // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - Accel_weight = constrain(1 - 2 * abs(1 - Accel_magnitude), 0, 1); // - - // We monitor the amount that the accelerometer based drift correction is deweighted for performanc reporting - imu_health = imu_health + 0.02 * (Accel_weight-.5); - imu_health = constrain(imu_health, 0, 1); - - Vector_Cross_Product(&errorRollPitch[0], &Accel_Vector[0], &DCM_Matrix[2][0]); // adjust the ground of reference - // errorRollPitch are in Accel ADC units - // Limit max errorRollPitch to limit max Omega_P and Omega_I - errorRollPitch[0] = constrain(errorRollPitch[0], -50, 50); - errorRollPitch[1] = constrain(errorRollPitch[1], -50, 50); - errorRollPitch[2] = constrain(errorRollPitch[2], -50, 50); - - Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight); - - Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight); - Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); - - //*****YAW*************** - - #if MAGNETOMETER == ENABLED - // We make the gyro YAW drift correction based on compass magnetic heading - errorCourse= (DCM_Matrix[0][0] * APM_Compass.Heading_Y) - (DCM_Matrix[1][0] * APM_Compass.Heading_X); // Calculating YAW error - #else // Use GPS Ground course to correct yaw gyro drift= - if(GPS.ground_speed >= SPEEDFILT){ - // Optimization: We have precalculated COGX and COGY (Course over Ground X and Y) from GPS info - errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); // Calculating YAW error - } - #endif - - Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); // Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - - Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW); - Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional. - - Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW); - Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I - - - // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros - Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I, Omega_I)); - if (Integrator_magnitude > ToRad(300)) { - Vector_Scale(Omega_I, Omega_I, 0.5f * ToRad(300) / Integrator_magnitude); - } -} - -/**************************************************/ -void Accel_adjust(void) -{ - Accel_Vector[1] += Accel_Scale((GPS.ground_speed / 100) * Omega[2]); // Centrifugal force on Acc_y = GPS_speed * GyroZ - Accel_Vector[2] -= Accel_Scale((GPS.ground_speed / 100) * Omega[1]); // Centrifugal force on Acc_z = GPS_speed * GyroY -} - - -/**************************************************/ -void Matrix_update(void) -{ - Gyro_Vector[0] = Gyro_Scaled_X(read_adc(0)); // gyro x roll - Gyro_Vector[1] = Gyro_Scaled_Y(read_adc(1)); // gyro y pitch - Gyro_Vector[2] = Gyro_Scaled_Z(read_adc(2)); // gyro Z yaw - - //Record when you saturate any of the gyros. - if((abs(Gyro_Vector[0]) >= ToRad(300)) || (abs(Gyro_Vector[1]) >= ToRad(300)) || (abs(Gyro_Vector[2]) >= ToRad(300))) - gyro_sat_count++; - -/* -Serial.print (AN[0]); -Serial.print (" "); -Serial.print (AN_OFFSET[0]); -Serial.print (" "); -Serial.print (Gyro_Vector[0]); -Serial.print (" "); -Serial.print (AN[1]); -Serial.print (" "); -Serial.print (AN_OFFSET[1]); -Serial.print (" "); -Serial.print (Gyro_Vector[1]); -Serial.print (" "); -Serial.print (AN[2]); -Serial.print (" "); -Serial.print (AN_OFFSET[2]); -Serial.print (" "); -Serial.println (Gyro_Vector[2]); -*/ - -// Accel_Vector[0]=read_adc(3); // acc x -// Accel_Vector[1]=read_adc(4); // acc y -// Accel_Vector[2]=read_adc(5); // acc z - // Low pass filter on accelerometer data (to filter vibrations) - Accel_Vector[0] = Accel_Vector[0] * 0.6 + (float)read_adc(3) * 0.4; // acc x - Accel_Vector[1] = Accel_Vector[1] * 0.6 + (float)read_adc(4) * 0.4; // acc y - Accel_Vector[2] = Accel_Vector[2] * 0.6 + (float)read_adc(5) * 0.4; // acc z - - - Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); // adding proportional term - Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); // adding Integrator term - - Accel_adjust(); // Remove centrifugal acceleration. - - #if OUTPUTMODE == 1 - Update_Matrix[0][0] = 0; - Update_Matrix[0][1] = -G_Dt * Omega_Vector[2]; // -z - Update_Matrix[0][2] = G_Dt * Omega_Vector[1]; // y - Update_Matrix[1][0] = G_Dt * Omega_Vector[2]; // z - Update_Matrix[1][1] = 0; - Update_Matrix[1][2] = -G_Dt * Omega_Vector[0]; // -x - Update_Matrix[2][0] = -G_Dt * Omega_Vector[1]; // -y - Update_Matrix[2][1] = G_Dt * Omega_Vector[0]; // x - Update_Matrix[2][2] = 0; - #else // Uncorrected data (no drift correction) - Update_Matrix[0][0] = 0; - Update_Matrix[0][1] = -G_Dt * Gyro_Vector[2]; // -z - Update_Matrix[0][2] = G_Dt * Gyro_Vector[1]; // y - Update_Matrix[1][0] = G_Dt * Gyro_Vector[2]; // z - Update_Matrix[1][1] = 0; - Update_Matrix[1][2] = -G_Dt * Gyro_Vector[0]; - Update_Matrix[2][0] = -G_Dt * Gyro_Vector[1]; - Update_Matrix[2][1] = G_Dt * Gyro_Vector[0]; - Update_Matrix[2][2] = 0; - #endif - - Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); // a * b = c - - for(int x = 0; x < 3; x++){ // Matrix Addition (update) - for(int y = 0; y < 3; y++){ - DCM_Matrix[x][y] += Temporary_Matrix[x][y]; - } - } - -/* -Serial.print (G_Dt * 1000); -Serial.print (" "); -Serial.print (DCM_Matrix[0][0]); -Serial.print (" "); -Serial.print (DCM_Matrix[0][1]); -Serial.print (" "); -Serial.print (DCM_Matrix[0][2]); -Serial.print (" "); -Serial.print (DCM_Matrix[1][0]); -Serial.print (" "); -Serial.print (DCM_Matrix[1][1]); -Serial.print (" "); -Serial.print (DCM_Matrix[1][2]); -Serial.print (" "); -Serial.print (DCM_Matrix[2][0]); -Serial.print (" "); -Serial.print (DCM_Matrix[2][1]); -Serial.print (" "); -Serial.println (DCM_Matrix[2][2]); -*/ -} - -/**************************************************/ -void Euler_angles(void) -{ - #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes) - roll = atan2(Accel_Vector[1], Accel_Vector[2]); // atan2(acc_y, acc_z) - pitch = -asin((Accel_Vector[0]) / (double)GRAVITY); // asin(acc_x) - yaw = 0; - roll_sensor = ToDeg(roll) * 100; - pitch_sensor = ToDeg(pitch) * 100; - #else - pitch = -asin(DCM_Matrix[2][0]); - roll = atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]); - yaw = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]); - pitch_sensor = ToDeg(pitch) * 100; - roll_sensor = ToDeg(roll) * 100; - yaw_sensor = ToDeg(yaw) * 100; - if (yaw_sensor < 0) yaw_sensor += 36000; - #endif - - /* - Serial.print ("Roll "); - Serial.print (roll_sensor / 100); - Serial.print (", Pitch "); - Serial.print (pitch_sensor / 100); - Serial.print (", Yaw "); - Serial.println (yaw_sensor / 100); - */ -} - -/**************************************************/ -//Computes the dot product of two vectors -float Vector_Dot_Product(float vector1[3], float vector2[3]) -{ - float op = 0; - - for(int c = 0; c < 3; c++) - { - op += vector1[c] * vector2[c]; - } - - return op; -} - -/**************************************************/ -//Computes the cross product of two vectors -void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]) -{ - vectorOut[0]= (v1[1] * v2[2]) - (v1[2] * v2[1]); - vectorOut[1]= (v1[2] * v2[0]) - (v1[0] * v2[2]); - vectorOut[2]= (v1[0] * v2[1]) - (v1[1] * v2[0]); -} - -/**************************************************/ -//Multiply the vector by a scalar. -void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2) -{ - for(int c = 0; c < 3; c++) - { - vectorOut[c] = vectorIn[c] * scale2; - } -} - -/**************************************************/ -void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]) -{ - for(int c = 0; c < 3; c++) - { - vectorOut[c] = vectorIn1[c]+vectorIn2[c]; - } -} - -/********* MATRIX FUNCTIONS *****************************************/ -//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). -void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3]) -{ - float op[3]; - for(int x = 0; x < 3; x++){ - for(int y = 0; y < 3; y++){ - for(int w = 0; w < 3; w++){ - op[w] = a[x][w] * b[w][y]; - } - mat[x][y] = 0; - mat[x][y] = op[0] + op[1] + op[2]; - float test = mat[x][y]; - } - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt deleted file mode 100644 index b7586d293d..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt +++ /dev/null @@ -1,389 +0,0 @@ -0 0000 16 int radio_trim 1 -1 0001 .. -2 0002 16 int radio_trim 2 -3 0003 .. -4 0004 16 int radio_trim 3 -5 0005 .. -6 0006 16 int radio_trim 4 -7 0007 .. -8 0008 16 int radio_trim 5 -9 0009 .. -10 000A 16 int radio_trim 6 -11 000B .. -12 000C 16 int radio_trim 7 -13 000D .. -14 000E 16 int radio_trim 8 -15 000F .. -16 0010 16 int radio_min 1 -17 0011 .. -18 0012 16 int radio_min 2 -19 0013 .. -20 0014 16 int radio_min 3 -21 0015 .. -22 0016 16 int radio_min 4 -23 0017 .. -24 0018 16 int radio_min 5 -25 0019 .. -26 001A 16 int radio_min 6 -27 001B .. -28 001C 16 int radio_min 7 -29 001D .. -30 001E 16 int radio_min 8 -31 001F .. -32 0020 16 int radio_max 1 -33 0021 .. -34 0022 16 int radio_max 2 -35 0023 .. -36 0024 16 int radio_max 3 -37 0025 .. -38 0026 16 int radio_max 4 -39 0027 .. -40 0028 16 int radio_max 5 -41 0029 .. -42 002A 16 int radio_max 6 -43 002B .. -44 002C 16 int radio_max 7 -45 002D .. -46 002E 16 int radio_max 8 -47 002F .. -48 0030 16 int elevon_trim 1 -49 0031 .. -50 0032 16 int elevon_trim 2 -51 0033 .. -52 0034 16 int XTRACK_GAIN -53 0035 .. -54 0036 16 int XTRACK_ENTRY_ANGLE -55 0037 .. -56 0038 8 int HEAD_MAX -57 0039 8 int PITCH_MAX -58 003A 8 int Pitch_min -59 003B 32 float EE_ALT_MIX -60 003C .. -61 003D .. -62 003E .. -63 003F -64 0040 32 float Kp 0 -65 0041 .. -66 0042 .. -67 0043 .. -68 0044 32 float Kp 1 -69 0045 .. -70 0046 .. -71 0047 .. -72 0048 32 float Kp 2 -73 0049 .. -74 004A .. -75 004B .. -76 004C 32 float Kp 3 -77 004D .. -78 004E .. -79 004F .. -80 0050 32 float Kp 4 -81 0051 .. -82 0052 .. -83 0053 .. -84 0054 32 float Kp 5 -85 0055 .. -86 0056 .. -87 0057 .. -88 0058 32 float Kp 6 -89 0059 .. -90 005A .. -91 005B .. -92 005C 32 float Kp 7 -93 005D .. -94 005E .. -95 005F .. -96 0060 32 float Ki 0 -97 0061 .. -98 0062 .. -99 0063 .. -100 0064 32 float Ki 1 -101 0065 .. -102 0066 .. -103 0067 .. -104 0068 32 float Ki 2 -105 0069 .. -106 006A .. -107 006B .. -108 006C 32 float Ki 3 -109 006D .. -110 006E .. -111 006F .. -112 0070 32 float Ki 4 -113 0071 .. -114 0072 .. -115 0073 .. -116 0074 32 float Ki 5 -117 0075 .. -118 0076 .. -119 0077 .. -120 0078 32 float Ki 6 -121 0079 .. -122 007A .. -123 007B .. -124 007C 32 float Ki 7 -125 007D .. -126 007E .. -127 007F .. -128 0080 32 float kd 0 -129 0081 .. -130 0082 .. -131 0083 .. -132 0084 32 float kd 1 -133 0085 .. -134 0086 .. -135 0087 .. -136 0088 32 float kd 2 -137 0089 .. -138 008A .. -139 008B .. -140 008C 32 float kd 3 -141 008D .. -142 008E .. -143 008F .. -144 0090 32 float kd 4 -145 0091 .. -146 0092 .. -147 0093 .. -148 0094 32 float kd 5 -149 0095 .. -150 0096 .. -151 0097 .. -152 0098 32 float kd 6 -153 0099 .. -154 009A .. -155 009B .. -156 009C 32 float kd 7 -157 009D .. -158 009E .. -159 009F .. -160 00A0 32 float integrator_max 0 -161 00A1 .. -162 00A2 .. -163 00A3 .. -164 00A4 32 float integrator_max 1 -165 00A5 .. -166 00A6 .. -167 00A7 .. -168 00A8 32 float integrator_max 2 -169 00A9 .. -170 00AA .. -171 00AB .. -172 00AC 32 float integrator_max 3 -173 00AD .. -174 00AE .. -175 00AF .. -176 00B0 32 float integrator_max 4 -177 00B1 .. -178 00B2 .. -179 00B3 .. -180 00B4 32 float integrator_max 5 -181 00B5 .. -182 00B6 .. -183 00B7 .. -184 00B8 32 float integrator_max 6 -185 00B9 .. -186 00BA .. -187 00BB .. -188 00BC 32 float integrator_max 7 -189 00BD .. -190 00BE .. -191 00BF .. -192 00C0 32 float Kff 0 -193 00C1 .. -194 00C2 .. -195 00C3 .. -196 00C4 32 float Kff 1 -197 00C5 .. -198 00C6 .. -199 00C7 .. -200 00C8 32 float Kff 2 -201 00C9 .. -202 00CA .. -203 00CB .. -204 00CC 32 float Kff 3 -205 00CD .. -206 00CE .. -207 00CF .. -208 00D0 32 float Kff 4 -209 00D1 .. -210 00D2 .. -211 00D3 .. -212 00D4 32 float Kff 5 -213 00D5 .. -214 00D6 .. -215 00D7 .. -216 00D8 32 float Kff 6 -217 00D9 .. -218 00DA .. -219 00DB .. -220 00DC 32 float Kff 7 -221 00DD .. -222 00DE .. -223 00DF .. -224 00E0 32 int EE_ACCEL_OFFSET 0 -225 00E1 .. -226 00E2 .. -227 00E3 .. -228 00E4 32 int EE_ACCEL_OFFSET 1 -229 00E5 .. -230 00E6 .. -231 00E7 .. -232 00E8 32 int EE_ACCEL_OFFSET 2 -233 00E9 .. -234 00EA .. -235 00EB .. -236 00EC 32 int EE_ACCEL_OFFSET 3 -237 00ED .. -238 00EE .. -239 00EF .. -240 00F0 32 int EE_ACCEL_OFFSET 4 -241 00F1 .. -242 00F2 .. -243 00F3 .. -244 00F4 32 int EE_ACCEL_OFFSET 5 -245 00F5 .. -246 00F6 .. -247 00F7 .. -248 00F8 8 EE_CONFIG -249 00F9 8 EE_WP_MODE -250 00FA 8 EE_YAW_MODE -251 00FB 8 EE_WP_TOTAL 0x106 -252 00FC 8 EE_WP_INDEX 0x107 -253 00FD 8 EE_WP_RADIUS 0x108 -254 00FE 8 EE_LOITER_RADIUS 0x109 -255 00FF 32 EE_ALT_HOLD_HOME 0x10A -256 0100 .. -257 0101 .. -258 0102 .. -259 0103 8 AIRSPEED_CRUISE -260 0104 32 AIRSPEED_RATIO -261 0105 .. -262 0106 .. -263 0107 .. -264 0108 AIRSPEED_FBW_MIN -265 0109 AIRSPEED_FBW_MAX -266 010A 8 THROTTLE_MIN -267 010B 8 THROTTLE_CRUISE -268 010C 8 THROTTLE_MAX -269 010D 8 THROTTLE_FAILSAFE -270 010E 16 THROTTLE_FS_VALUE -271 010F .. -272 0110 8 THROTTLE_FAILSAFE_ACTION -273 0111 -274 0112 8 FLIGHT_MODE_CHANNEL -275 0113 8 AUTO_TRIM -276 0114 uint16 LOGging Bitmask -277 0115 .. -278 0116 uint32 EE_ABS_PRESS_GND -279 0117 .. -280 0118 .. -281 0119 .. -282 011A 16 int EE_GND_TEMP -283 011B .. -284 011C 16 int EE_GND_ALT -285 011D .. -286 011E 16 int EE_AP_OFFSET -287 011F .. -288 0120 8 byte EE_SWITCH_ENABLE -289 0121 8 ARRAY FLIGHT_MODES_1 -290 0122 8 ARRAY FLIGHT_MODES_2 -291 0123 8 ARRAY FLIGHT_MODES_3 -292 0124 8 ARRAY FLIGHT_MODES_4 -293 0125 8 ARRAY FLIGHT_MODES_5 -294 0126 8 ARRAY FLIGHT_MODES_6 -295 0127 -296 0128 -297 0129 -298 012A -299 012B -300 012C -301 012D -302 012E -303 012F (0x130 is the Start Byte for commands) -304 0130 byte Command id (Home) 0 -305 0131 byte Parameter 1 -306 0132 long Altitude, Parameter 2 -307 0133 .. -308 0134 .. -309 0135 .. -310 0136 long Latitude, Parameter 3 -311 0137 .. -312 0138 .. -313 0139 .. -314 013A long Longitude, Parameter 3 -315 013B .. -316 013C .. -317 013D byte Command id (WP 1) 1 -318 013E byte Parameter 1 -319 013F long Altitude, Parameter 2 -320 0140 .. -321 0141 .. -322 0142 .. -323 0143 long Latitude, Parameter 3 -324 0144 .. -325 0145 .. -326 0146 .. -327 0147 long Longitude, Parameter 3 -328 0148 .. -329 0149 .. -330 014A .. -331 014B -332 014C -333 014D -334 014E -335 014F - - - - - -********************************************************************************************** -SECTION 6 - LOGS - -3584 0xE00 int Last log page used -3585 0xE01 .. -3587 0xE02 byte last log number -3588 0xE03 unused -3589 0xE04 int Log 1 start page -3590 0xE05 .. -3591 0xE06 int Log 2 start page -3592 0xE07 .. -3593 0xE08 int Log 3 start page -3594 0xE09 .. -3595 0xE0A int Log 4 start page -3596 0xE0B .. -3597 0xE0C int Log 5 start page -3598 0xE0D .. -3599 0xE0E int Log 6 start page -3600 0xE0F .. -3601 0xE10 int Log 7 start page -3602 0xE11 .. -3603 0xE12 int Log 8 start page -3604 0xE13 .. -3605 0xE14 int Log 9 start page -3606 0xE15 .. -3607 0xE16 int Log 10 start page -3608 0xE17 .. -3609 0xE18 int Log 11 start page -3610 0xE19 .. -3611 0xE1A int Log 12 start page -3612 0xE1B .. -3613 0xE1C int Log 13 start page -3614 0xE1D .. -3615 0xE1E int Log 14 start page -3616 0xE1F .. -3617 0xE20 int Log 15 start page -3618 0xE21 .. -3619 0xE22 int Log 16 start page -3620 0xE23 .. -3621 0xE24 int Log 17 start page -3622 0xE25 .. -3623 0xE26 int Log 18 start page -3624 0xE27 .. -3625 0xE28 int Log 19 start page -3626 0xE29 .. - - - \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde deleted file mode 100644 index 9cf861f3bc..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde +++ /dev/null @@ -1,286 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// ************************************************************************************ -// This function gets critical data from EEPROM to get us underway if restarting in air -// ************************************************************************************ -void read_EEPROM_startup(void) -{ - read_EEPROM_gains(); - read_EEPROM_radio_minmax(); // read Radio limits - read_EEPROM_trims(); // read Radio trims - read_user_configs(); - read_EEPROM_waypoint_info(); -} - -void read_EEPROM_airstart_critical(void) -{ - int16_t temp = 0; - read_EEPROM_IMU_offsets(); - - // For debugging only - /* - Serial.print ("Offsets "); Serial.print (AN_OFFSET[0]); - Serial.print (" "); Serial.print (AN_OFFSET[1]); - Serial.print (" "); Serial.print (AN_OFFSET[2]); - Serial.print (" "); Serial.print (AN_OFFSET[3]); - Serial.print (" "); Serial.print (AN_OFFSET[4]); - Serial.print (" "); Serial.println (AN_OFFSET[5]); - */ - - // Read the home location - //----------------------- - home = get_wp_with_index(0); - - // Read pressure sensor values - //---------------------------- - read_pressure_data(); -} - -void save_EEPROM_groundstart(void) -{ - save_EEPROM_trims(); - save_EEPROM_IMU_offsets(); - // pressure sensor data saved by init_home -} - - -/********************************************************************************/ -long read_alt_to_hold() -{ - byte options = eeprom_read_byte((byte *) EE_CONFIG); - - // save the alitude above home option - if(options & HOLD_ALT_ABOVE_HOME){ - int32_t temp = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME); - return temp + home.alt; - }else{ - return current_loc.alt; - } -} - -long save_alt_to_hold(int32_t alt_to_hold) -{ - byte options = eeprom_read_byte((byte *) EE_CONFIG); - - // save the alitude above home option - if(options & HOLD_ALT_ABOVE_HOME) - eeprom_write_block((const void *)&alt_to_hold, (void *)EE_ALT_HOLD_HOME, sizeof (alt_to_hold)); -} - - -/********************************************************************************/ -void read_EEPROM_waypoint_info(void) -{ - wp_total = eeprom_read_byte((uint8_t *) EE_WP_TOTAL); - wp_radius = eeprom_read_byte((uint8_t *) EE_WP_RADIUS); - loiter_radius = eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS); -} - -void save_EEPROM_waypoint_info(void) -{ - eeprom_write_byte((uint8_t *) EE_WP_TOTAL, wp_total); - eeprom_write_byte((uint8_t *) EE_WP_RADIUS, wp_radius); - eeprom_write_byte((uint8_t *) EE_LOITER_RADIUS, loiter_radius); -} - -/********************************************************************************/ -void read_EEPROM_gains(void) -{ - eeprom_read_block((void*)&kp, (const void*)EE_KP, sizeof(kp)); - eeprom_read_block((void*)&ki, (const void*)EE_KI, sizeof(ki)); - eeprom_read_block((void*)&kd, (const void*)EE_KD, sizeof(kd)); - eeprom_read_block((void*)&integrator_max, (const void*)EE_IMAX, sizeof(integrator_max)); - eeprom_read_block((void*)&kff, (const void*)EE_KFF, sizeof(kff)); - - // stored as degree * 100 - x_track_gain = eeprom_read_word((uint16_t *) EE_XTRACK_GAIN); - - // stored as degrees - x_track_angle = eeprom_read_word((uint16_t *) EE_XTRACK_ANGLE) * 100; - - // stored as degrees - head_max = eeprom_read_byte((uint8_t *) EE_HEAD_MAX) * 100; // scale to degress * 100 - pitch_max = eeprom_read_byte((uint8_t *) EE_PITCH_MAX) * 100; // scale to degress * 100 - pitch_min = -eeprom_read_byte((uint8_t *) EE_PITCH_MIN) * 100; // scale to degress * 100 - - // stored as a float - eeprom_read_block((void*)&altitude_mix, (const void*)EE_ALT_MIX, sizeof(altitude_mix)); -} - -void save_EEPROM_gains(void) -{ - eeprom_write_block((const void *)&kp, (void *)EE_KP, sizeof (kp)); - eeprom_write_block((const void *)&ki, (void *)EE_KI, sizeof (ki)); - eeprom_write_block((const void *)&kd, (void *)EE_KD, sizeof (kd)); - eeprom_write_block((const void *)&integrator_max, (void *)EE_IMAX, sizeof (integrator_max)); - eeprom_write_block((const void *)&kff, (void *)EE_KFF, sizeof (kff)); - - // stored as degree * 100 - eeprom_write_word((uint16_t *) EE_XTRACK_GAIN, x_track_gain); - - // stored as degrees - eeprom_write_word((uint16_t *) EE_XTRACK_ANGLE, x_track_angle / 100); - - // stored as degrees - eeprom_write_byte((uint8_t *) EE_HEAD_MAX, head_max / 100); - eeprom_write_byte((uint8_t *) EE_PITCH_MAX, pitch_max / 100); - eeprom_write_byte((uint8_t *) EE_PITCH_MIN, -pitch_min / 100); - - // stored as a float - eeprom_write_block((const void*)&altitude_mix, (void*)EE_ALT_MIX, sizeof(altitude_mix)); -} - - -/********************************************************************************/ - -void read_EEPROM_trims(void) -{ - // Read Radio trim settings - eeprom_read_block((void*)&radio_trim, (const void*)EE_TRIM, sizeof(radio_trim)); - elevon1_trim = eeprom_read_word((uint16_t *) EE_ELEVON1_TRIM); - elevon2_trim = eeprom_read_word((uint16_t *) EE_ELEVON2_TRIM); -} - -void save_EEPROM_trims(void) -{ - // Save Radio trim settings - eeprom_write_block((const void *)&radio_trim, (void *)EE_TRIM, sizeof(radio_trim)); - eeprom_write_word((uint16_t *) EE_ELEVON1_TRIM, elevon1_trim); - eeprom_write_word((uint16_t *) EE_ELEVON2_TRIM, elevon2_trim); -} - -/********************************************************************************/ - -void save_EEPROM_IMU_offsets(void) -{ - eeprom_write_block((const void *)&AN_OFFSET, (void *)EE_AN_OFFSET, sizeof (AN_OFFSET)); -} - -void read_EEPROM_IMU_offsets(void) -{ - eeprom_read_block((void*)&AN_OFFSET, (const void*)EE_AN_OFFSET, sizeof(AN_OFFSET)); -} - -/********************************************************************************/ - -void save_command_index(void) -{ - eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index); -} - -void read_command_index(void) -{ - wp_index = command_must_index = eeprom_read_byte((uint8_t *) EE_WP_INDEX); -} - -/********************************************************************************/ - -void save_pressure_data(void) -{ - eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, abs_press_gnd); - eeprom_write_word((uint16_t *)EE_GND_TEMP, ground_temperature); - eeprom_write_word((uint16_t *)EE_GND_ALT, (ground_alt / 100)); - - #if AIRSPEED_SENSOR == 1 - eeprom_write_word((uint16_t *)EE_AP_OFFSET, airpressure_offset); - #endif -} - -void read_pressure_data(void) -{ - abs_press_gnd = eeprom_read_dword((uint32_t *) EE_ABS_PRESS_GND); - abs_press_filt = abs_press_gnd; // Better than zero for an air start value - ground_temperature = eeprom_read_word((uint16_t *) EE_GND_TEMP); - ground_alt = eeprom_read_word((uint16_t *) EE_GND_ALT) * 100; - - #if AIRSPEED_SENSOR == 1 - airpressure_offset = eeprom_read_word((uint16_t *) EE_AP_OFFSET); - #endif -} - - - -/********************************************************************************/ - -void read_EEPROM_radio_minmax(void) -{ - // Read Radio min/max settings - eeprom_read_block((void*)&radio_max, (const void*)EE_MAX, sizeof(radio_max)); - eeprom_read_block((void*)&radio_min, (const void*)EE_MIN, sizeof(radio_min)); -} - - -void save_EEPROM_radio_minmax(void) -{ - // Save Radio min/max settings - eeprom_write_block((const void *)&radio_max, (void *)EE_MAX, sizeof(radio_max)); - eeprom_write_block((const void *)&radio_min, (void *)EE_MIN, sizeof(radio_min)); -} - - - -/********************************************************************************/ - -void read_user_configs(void) -{ - // Read Radio min/max settings - airspeed_cruise = eeprom_read_byte((byte *) EE_AIRSPEED_CRUISE)*100; - eeprom_read_block((void*)&airspeed_ratio, (const void*)EE_AIRSPEED_RATIO, sizeof(airspeed_ratio)); - - throttle_min = eeprom_read_byte((byte *) EE_THROTTLE_MIN); - throttle_max = eeprom_read_byte((byte *) EE_THROTTLE_MAX); - throttle_cruise = eeprom_read_byte((byte *) EE_THROTTLE_CRUISE); - - airspeed_fbw_min = eeprom_read_byte((byte *) EE_AIRSPEED_FBW_MIN); - airspeed_fbw_max = eeprom_read_byte((byte *) EE_AIRSPEED_FBW_MAX); - - throttle_failsafe_enabled = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE); - throttle_failsafe_action = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION); - throttle_failsafe_value = eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE); - - //flight_mode_channel = eeprom_read_byte((byte *) EE_FLIGHT_MODE_CHANNEL); - auto_trim = eeprom_read_byte((byte *) EE_AUTO_TRIM); - log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK); - - read_EEPROM_flight_modes(); - reverse_switch = eeprom_read_byte((byte *) EE_REVERSE_SWITCH); -} - - -void save_user_configs(void) -{ - eeprom_write_byte((byte *) EE_AIRSPEED_CRUISE, airspeed_cruise/100); - eeprom_write_block((const void *)&airspeed_ratio, (void *)EE_AIRSPEED_RATIO, sizeof(airspeed_ratio)); - - eeprom_write_byte((byte *) EE_THROTTLE_MIN, throttle_min); - eeprom_write_byte((byte *) EE_THROTTLE_MAX, throttle_max); - eeprom_write_byte((byte *) EE_THROTTLE_CRUISE, throttle_cruise); - - eeprom_write_byte((byte *) EE_AIRSPEED_FBW_MIN, airspeed_fbw_min); - eeprom_write_byte((byte *) EE_AIRSPEED_FBW_MAX, airspeed_fbw_max); - - eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE, throttle_failsafe_enabled); - eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action); - eeprom_write_word((uint16_t *) EE_THROTTLE_FS_VALUE, throttle_failsafe_value); - - //eeprom_write_byte((byte *) EE_FLIGHT_MODE_CHANNEL, flight_mode_channel); - eeprom_write_byte((byte *) EE_AUTO_TRIM, auto_trim); - eeprom_write_word((uint16_t *) EE_LOG_BITMASK, log_bitmask); - - //save_EEPROM_flight_modes(); - eeprom_write_byte((byte *) EE_REVERSE_SWITCH, reverse_switch); -} - -/********************************************************************************/ -void read_EEPROM_flight_modes(void) -{ - // Read Radio min/max settings - eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes)); -} - - -void save_EEPROM_flight_modes(void) -{ - // Save Radio min/max settings - eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes)); -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde deleted file mode 100644 index 4cc892f32c..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde +++ /dev/null @@ -1,176 +0,0 @@ - -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY - -#if GCS_PORT == 3 -# define SendSer Serial3.print -# define SendSerln Serial3.println -#else -# define SendSer Serial.print -# define SendSerln Serial.println -#endif - -// This is the standard GCS output file for ArduPilot - -/* -Message Prefixes -!!! Position Low rate telemetry -+++ Attitude High rate telemetry -### Mode Change in control mode -%%% Waypoint Current and previous waypoints -XXX Alert Text alert - NOTE: Alert message generation is not localized to a function -PPP IMU Performance Sent every 20 seconds for performance monitoring - -Message Suffix -*** All messages use this suffix -*/ - -/* -void acknowledge(byte id, byte check1, byte check2) {} -void send_message(byte id) {} -void send_message(byte id, long param) {} -void send_message(byte severity, const char *str) {} -*/ - -void acknowledge(byte id, byte check1, byte check2) -{ -} - -void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 -{ - if(severity >= DEBUG_LEVEL){ - SendSer("MSG: "); - SendSerln(str); - } -} - -void send_message(byte id) -{ - send_message(id,0l); -} - -void send_message(byte id, long param) -{ - switch(id) { - case MSG_ATTITUDE: // ** Attitude message - print_attitude(); - break; - case MSG_LOCATION: // ** Location/GPS message - print_position(); - break; - case MSG_HEARTBEAT: // ** Location/GPS message - print_control_mode(); - break; - //case MSG_PERF_REPORT: - // printPerfData(); - } -} - -void print_current_waypoints() -{ -} - -void print_attitude(void) -{ - SendSer("+++"); - SendSer("ASP:"); - SendSer((int)airspeed / 100, DEC); - SendSer(",THH:"); - SendSer(servo_out[CH_THROTTLE], DEC); - SendSer (",RLL:"); - SendSer(roll_sensor / 100, DEC); - SendSer (",PCH:"); - SendSer(pitch_sensor / 100, DEC); - SendSerln(",***"); -} - -void print_control_mode(void) -{ - switch (control_mode){ - case MANUAL: - SendSerln("###MANUAL***"); - break; - case STABILIZE: - SendSerln("###STABILIZE***"); - break; - case CIRCLE: - SendSerln("###CIRCLE***"); - break; - case FLY_BY_WIRE_A: - SendSerln("###FBW A***"); - break; - case FLY_BY_WIRE_B: - SendSerln("###FBW B***"); - break; - case AUTO: - SendSerln("###AUTO***"); - break; - case RTL: - SendSerln("###RTL***"); - break; - case LOITER: - SendSerln("###LOITER***"); - break; - case TAKEOFF: - SendSerln("##TAKEOFF***"); - break; - case LAND: - SendSerln("##LAND***"); - break; - } -} - -void print_position(void) -{ - SendSer("!!!"); - SendSer("LAT:"); - SendSer(current_loc.lat/10,DEC); - SendSer(",LON:"); - SendSer(current_loc.lng/10,DEC); //wp_current_lat - SendSer(",SPD:"); - SendSer(GPS.ground_speed/100,DEC); - SendSer(",CRT:"); - SendSer(climb_rate,DEC); - SendSer(",ALT:"); - SendSer(current_loc.alt/100,DEC); - SendSer(",ALH:"); - SendSer(next_WP.alt/100,DEC); - SendSer(",CRS:"); - SendSer(yaw_sensor/100,DEC); - SendSer(",BER:"); - SendSer(target_bearing/100,DEC); - SendSer(",WPN:"); - SendSer(wp_index,DEC);//Actually is the waypoint. - SendSer(",DST:"); - SendSer(wp_distance,DEC); - SendSer(",BTV:"); - SendSer(battery_voltage,DEC); - SendSer(",RSP:"); - SendSer(servo_out[CH_ROLL]/100,DEC); - SendSer(",TOW:"); - SendSer(GPS.time); - SendSerln(",***"); -} - -void print_waypoint(struct Location *cmd,byte index) -{ - SendSer("command #: "); - SendSer(index, DEC); - SendSer(" id: "); - SendSer(cmd->id,DEC); - SendSer(" p1: "); - SendSer(cmd->p1,DEC); - SendSer(" p2: "); - SendSer(cmd->alt,DEC); - SendSer(" p3: "); - SendSer(cmd->lat,DEC); - SendSer(" p4: "); - SendSerln(cmd->lng,DEC); -} - -void print_waypoints() -{ -} - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde deleted file mode 100644 index 8bd08bc1b2..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde +++ /dev/null @@ -1,1258 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL - -/* This GCS protocol sends text-base information over the telemetry port -*/ - -#include //Allow us to store string constants (there are a lot!) in program memory to avoid using up valuable RAM - -#define ERR(a) ((DEBUGTERMINAL_VERBOSE)>0?(PSTR(a)):(0)) - -#if GCS_PORT == 3 -# define DTSTREAM Serial3 -#else -# define DTSTREAM Serial -#endif - -//Read buffer -#define DEBUGTERM_BUFFERSIZE (120) -char gcsinputbuffer[DEBUGTERM_BUFFERSIZE]; -byte bufferidx; - -//Reporting flags -byte report_heartbeat = 1; -byte report_attitude = 0; -byte report_location = 0; -byte report_command = 1; -byte report_severity = 0; -byte first_location = 0; - -void readgcsinput() { - byte numc, i, c; - - numc = DTSTREAM.available(); - for (i=0;i 0) bufferidx--; - } else if (c==10) { - //Received a linefeed; do nothing - } else if (c==13) { - //Received a carriage return; process command - gcsinputbuffer[bufferidx] = 0; - run_debugt_command(gcsinputbuffer); - bufferidx = 0; - } else { - gcsinputbuffer[bufferidx++] = c; - if (bufferidx >= DEBUGTERM_BUFFERSIZE) bufferidx = 0; - } -} - -void run_debugt_command(char *buf) { - //*********** Ignore comments *********** - if (strmatch(buf, PSTR("//"))) { - - //*********** Process 'show' commands *********** - } else if (strmatch(buf, PSTR("show "))) { - if (strmatch(buf+5, PSTR("heartbeat"))) - report_heartbeat = 1; - else if (strmatch(buf+5, PSTR("attitude"))) - report_attitude = 1; - else if (strmatch(buf+5, PSTR("location"))) - report_location = 1; - else if (strmatch(buf+5, PSTR("command"))) - report_command = 1; - else if (strmatch(buf+5, PSTR("severity"))) - report_severity = atoi(buf+14); - else - print_error(ERR("USAGE: show {heartbeat|attitude|location|command|severity }")); - - //*********** Process 'hide' commands *********** - } else if (strmatch(buf, PSTR("hide "))) { - if (strmatch(buf+5, PSTR("heartbeat"))) - report_heartbeat = 0; - else if (strmatch(buf+5, PSTR("attitude"))) - report_attitude = 0; - else if (strmatch(buf+5, PSTR("location"))) - report_location = 0; - else if (strmatch(buf+5, PSTR("command"))) - report_command = 0; - else if (strmatch(buf+5, PSTR("all"))) { - report_heartbeat = 0; - report_attitude = 0; - report_location = 0; - report_command = 0; - } else - print_error(ERR("USAGE: hide {heartbeat|attitude|location|command|all}")); - - //*********** Process 'echo' command *********** - } else if (strmatch(buf, PSTR("echo "))) { - DTSTREAM.println(buf+5); - - //*********** Process 'groundstart' command *********** - } else if (strmatch(buf, PSTR("groundstart"))) { - startup_ground(); - - //*********** Process 'inithome' command *********** - } else if (strmatch(buf, PSTR("inithome"))) { - init_home(); - DTSTREAM.println_P("Home set."); - - //*********** Process 'print' commands *********** - } else if (strmatch(buf, PSTR("print "))) { - //------- print altitude ------- - if (strmatch(buf+6, PSTR("altitude"))) { - DTSTREAM.println_P("Altitude:"); - DTSTREAM.print_P(" Pressure: "); DTSTREAM.print(((float)press_alt)/100,2); DTSTREAM.println_P(" m"); - DTSTREAM.print_P(" GPS: "); DTSTREAM.print(((float)GPS.altitude)/100,2); DTSTREAM.println_P(" m"); - DTSTREAM.print_P(" Mix ratio: "); DTSTREAM.println(altitude_mix,3); - DTSTREAM.print_P(" Mix: "); DTSTREAM.print((float)(((1 - altitude_mix) * GPS.altitude) + (altitude_mix * press_alt))/100,2); DTSTREAM.println_P(" m"); - - //------- print attitude ------- - } else if (strmatch(buf+6, PSTR("attitude"))) { - print_attitude(); - - //------- print commands ------- - } else if (strmatch(buf+6, PSTR("commands"))) { - print_waypoints(); - - //------- print ctrlmode ------- - } else if (strmatch(buf+6, PSTR("ctrlmode"))) { - print_control_mode(); - - //------- print curwaypts ------- - } else if (strmatch(buf+6, PSTR("curwaypts"))) { - print_current_waypoints(); - - //------- print flightmodes ------- - } else if (strmatch(buf+6, PSTR("flightmodes"))) { - int i; - DTSTREAM.print_P("EEPROM read: "); - for (i=0; i<6; i++) { - DTSTREAM.print_P("Ch "); DTSTREAM.print(i,DEC); DTSTREAM.print_P(" = "); DTSTREAM.print(flight_modes[i],DEC); DTSTREAM.print_P(", "); - } - DTSTREAM.println(" "); - - //------- print k -? ------- - } else if (strmatch(buf+6, PSTR("k -?"))) { - print_error(ERR("USAGE: print k{p|i|d} {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt}")); - print_error(ERR("USAGE: print kff {pitchcomp|ruddermix|pitchtothrottle}")); - - //------- print kp ------- - } else if (strmatch(buf+6, PSTR("kp "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+9, &i, &j)) { - DTSTREAM.print_P("P gain for "); - DTSTREAM.print(buf+9); - DTSTREAM.print_P(" = "); - DTSTREAM.println(kp[i],DEC); - } else - print_error(ERR("ERROR: Did not recognize P keyword; type print k -? for more information")); - - //------- print ki ------- - } else if (strmatch(buf+6, PSTR("ki "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+9, &i, &j)) { - DTSTREAM.print_P("I gain for "); - DTSTREAM.print(buf+9); - DTSTREAM.print_P(" = "); - DTSTREAM.println(ki[i],DEC); - } else - print_error(ERR("ERROR: Did not recognize I keyword; type print k -? for more information")); - - //------- print kd ------- - } else if (strmatch(buf+6, PSTR("kd "))) { - int i; - unsigned char j; - if (get_pid_offset_name(buf+9, &i, &j)) { - DTSTREAM.print_P("D gain for "); - DTSTREAM.print(buf+9); - DTSTREAM.print_P(" = "); - DTSTREAM.println(kd[i],DEC); - } else - print_error(ERR("ERROR: Did not recognize D keyword; type print k -? for more information")); - - //------- print kff ------- - } else if (strmatch(buf+6, PSTR("kff "))) { - if (strmatch(buf+10, PSTR("pitchcomp"))) { - DTSTREAM.print_P("FF gain for pitchcomp = "); - DTSTREAM.println(kff[CASE_PITCH_COMP],DEC); - } else if (strmatch(buf+10, PSTR("ruddermix"))) { - DTSTREAM.print_P("FF gain for ruddermix = "); - DTSTREAM.println(kff[CASE_RUDDER_MIX],DEC); - } else if (strmatch(buf+10, PSTR("pitchtothrottle"))) { - DTSTREAM.print_P("FF gain for pitchtothrottle = "); - DTSTREAM.println(kff[CASE_P_TO_T],DEC); - } else - print_error(ERR("ERROR: Did not recognize FF keyword; type print k -? for more information")); - - //------- print location ------- - } else if (strmatch(buf+6, PSTR("location"))) { - print_position(); - - //------- print navsettings ------- - } else if (strmatch(buf+6, PSTR("navsettings"))) { - DTSTREAM.println_P("Navigation settings:"); -#if AIRSPEED_SENSOR == 1 - DTSTREAM.print_P(" Cruise airspeed: "); DTSTREAM.println((float)airspeed_cruise/100.0,2); -#else - DTSTREAM.print_P(" Cruise throttle: "); DTSTREAM.println(throttle_cruise,DEC); -#endif - DTSTREAM.print_P(" Hold altitude above home: "); DTSTREAM.println(read_alt_to_hold(),DEC); - DTSTREAM.print_P(" Loiter radius: "); DTSTREAM.println(loiter_radius,DEC); - DTSTREAM.print_P(" Waypoint radius: "); DTSTREAM.println(wp_radius,DEC); - - //------- print pressure ------- - } else if (strmatch(buf+6, PSTR("pressure"))) { - DTSTREAM.println_P("BMP085 pressure sensor:"); - DTSTREAM.print_P(" Temperature: "); DTSTREAM.println(APM_BMP085.Temp,DEC); - DTSTREAM.print_P(" Pressure: "); DTSTREAM.println(APM_BMP085.Press,DEC); - - //------- print rlocation home ------- - } else if (strmatch(buf+6, PSTR("rlocation home"))) { - print_rlocation(&home); - - //------- print rlocation ------- - //(implication is "relative to next waypoint") - } else if (strmatch(buf+6, PSTR("rlocation"))) { - print_rlocation(&next_WP); - - //------- print speed ------- - } else if (strmatch(buf+6, PSTR("speed"))) { - DTSTREAM.println_P("Speed:"); - DTSTREAM.print_P(" Ground: "); DTSTREAM.println((float)GPS.ground_speed/100.0,2); -#if AIRSPEED_SENSOR == 1 - DTSTREAM.print_P(" Air: "); DTSTREAM.println((float)airspeed/100.0,2); - DTSTREAM.print_P(" Cruise: "); DTSTREAM.println((float)airspeed_cruise/100.0,2); -#endif - - //------- print tuning ------- - } else if (strmatch(buf+6, PSTR("tuning"))) { - print_tuning(); - - } else - print_error(ERR("USAGE: print {altitude|attitude|commands|ctrlmode|curwaypts|flightmodes|k -?|kp|ki|kd|kff|location|navsettings|pressure|rlocation [home]|speed|tuning|}")); - - //*********** Process 'reset' commands *********** - } else if (strmatch(buf, PSTR("reset "))) { - //------- reset commands ------- - if (strmatch(buf+6, PSTR("commands"))) { - reload_commands(); - DTSTREAM.println_P("Commands reloaded."); - } else - print_error(ERR("USAGE: reset commands")); - - //*********** Process 'rtl' command *********** - } else if (strmatch(buf, PSTR("rtl"))) { - return_to_launch(); - DTSTREAM.println_P("Returning to launch..."); - - //*********** Process 'set' commands *********** - } else if (strmatch(buf, PSTR("set "))) { - //------- set cmd ------- - if (strmatch(buf+4, PSTR("cmd "))) { - process_set_cmd(buf+8, bufferidx-8); - - //------- set cmdcount ------- - } else if (strmatch(buf+4, PSTR("cmdcount "))) { - wp_total = atoi(buf+13); - save_EEPROM_waypoint_info(); - DTSTREAM.print_P("wp_total = "); DTSTREAM.println(wp_total,DEC); - - //------- set cmdindex ------- - } else if (strmatch(buf+4, PSTR("cmdindex "))) { - wp_index = atoi(buf+13); - decrement_WP_index(); - next_command = get_wp_with_index(wp_index+1); - DTSTREAM.print_P("Command set to index "); DTSTREAM.print(wp_index,DEC); - if (next_command.id >= 0x10 && next_command.id <= 0x1F) { //TODO: create a function the defines what type of command each command ID is - command_must_index = 0; - DTSTREAM.println_P(" (must)"); - } else if (next_command.id >= 0x20 && next_command.id <= 0x2F) { - command_may_index = 0; - DTSTREAM.println_P(" (may)"); - } else - DTSTREAM.println_P(" (now)"); - - next_command.id = CMD_BLANK; - if (wp_index > wp_total) { - wp_total = wp_index; - DTSTREAM.print_P(" The total number of commands is now "); - DTSTREAM.println(wp_total,DEC); - } - update_commands(); - - //------- set cruise ------- - } else if (strmatch(buf+4, PSTR("cruise "))) { - unsigned char j = 4+7; -#if AIRSPEED_SENSOR == 1 - DTSTREAM.print_P("airspeed_cruise changed from "); - DTSTREAM.print((float)airspeed_cruise/100,2); - DTSTREAM.print_P(" to "); - airspeed_cruise = (int)(readfloat(buf, &j)/100000); - airspeed_cruise = constrain(airspeed_cruise,0,9000); //TODO: constrain minimum as stall speed, maximum as Vne - DTSTREAM.println(((float)airspeed_cruise)/100,2); -#else - DTSTREAM.print_P("throttle_cruise changed from "); - DTSTREAM.print(throttle_cruise,DEC); - DTSTREAM.print_P(" to "); - throttle_cruise = constrain((int)(readfloat(buf, &j)/10000000),0,200); - DTSTREAM.println(throttle_cruise,DEC); -#endif - - //------- set holdalt ------- - } else if (strmatch(buf+4, PSTR("holdalt "))) { - int tempalt = atoi(buf+12)*100; - save_alt_to_hold((int32_t)tempalt); - DTSTREAM.println_P("Hold altitude above home set."); - - //------- set k -? ------- - } else if (strmatch(buf+4, PSTR("k -?"))) { - print_error(ERR("USAGE: set k{p|i|d} {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} ")); - print_error(ERR("USAGE: set kff {pitchcomp|ruddermix|pitchtothrottle} ")); - - //------- set kp ------- - } else if (strmatch(buf+4, PSTR("kp "))) { - int i; - unsigned char j = 0; - if (get_pid_offset_name(buf+7, &i, &j)) { - buf[7+j] = 0; - DTSTREAM.print_P("P gain for "); - DTSTREAM.print(buf+7); - DTSTREAM.print_P(" changed from "); - DTSTREAM.print(kp[i],DEC); - DTSTREAM.print_P(" to "); - j += 7+1; - kp[i] = ((float)readfloat(buf, &j))/10000000; - DTSTREAM.println(kp[i],DEC); - } else - print_error(ERR("ERROR: Did not recognize P keyword; type set k -? for more information")); - - //------- set ki ------- - } else if (strmatch(buf+4, PSTR("ki "))) { - int i; - unsigned char j = 0; - if (get_pid_offset_name(buf+7, &i, &j)) { - buf[7+j] = 0; - DTSTREAM.print_P("I gain for "); - DTSTREAM.print(buf+7); - DTSTREAM.print_P(" changed from "); - DTSTREAM.print(ki[i],DEC); - DTSTREAM.print_P(" to "); - j += 7+1; - ki[i] = ((float)readfloat(buf, &j))/10000000; - DTSTREAM.println(ki[i],DEC); - } else - print_error(ERR("ERROR: Did not recognize I keyword; type set k -? for more information")); - - //------- set kd ------- - } else if (strmatch(buf+4, PSTR("kd "))) { - int i; - unsigned char j = 0; - if (get_pid_offset_name(buf+7, &i, &j)) { - buf[7+j] = 0; - DTSTREAM.print_P("D gain for "); - DTSTREAM.print(buf+7); - DTSTREAM.print_P(" changed from "); - DTSTREAM.print(kd[i],DEC); - DTSTREAM.print_P(" to "); - j += 7+1; - kd[i] = ((float)readfloat(buf, &j))/10000000; - DTSTREAM.println(kd[i],DEC); - } else - print_error(ERR("ERROR: Did not recognize D keyword; type set k -? for more information")); - - //------- set kff ------- - } else if (strmatch(buf+4, PSTR("kff "))) { - unsigned char j = 0; - if (strmatch(buf+8, PSTR("pitchcomp"))) { - buf[8+9] = 0; - DTSTREAM.print_P("FF gain for "); - DTSTREAM.print(buf+8); - DTSTREAM.print_P(" changed from "); - DTSTREAM.print(kff[CASE_PITCH_COMP],DEC); - DTSTREAM.print_P(" to "); - j = 8+9+1; - kff[CASE_PITCH_COMP] = ((float)readfloat(buf, &j))/10000000; - DTSTREAM.println(kff[CASE_PITCH_COMP],DEC); - } else if (strmatch(buf+8, PSTR("ruddermix"))) { - buf[8+9] = 0; - DTSTREAM.print_P("FF gain for "); - DTSTREAM.print(buf+8); - DTSTREAM.print_P(" changed from "); - DTSTREAM.print(kff[CASE_RUDDER_MIX],DEC); - DTSTREAM.print_P(" to "); - j = 8+9+1; - kff[CASE_RUDDER_MIX] = ((float)readfloat(buf, &j))/10000000; - DTSTREAM.println(kff[CASE_RUDDER_MIX],DEC); - } else if (strmatch(buf+8, PSTR("pitchtothrottle"))) { - buf[8+15] = 0; - DTSTREAM.print_P("FF gain for "); - DTSTREAM.print(buf+8); - DTSTREAM.print_P(" changed from "); - DTSTREAM.print(kff[CASE_P_TO_T],DEC); - DTSTREAM.print_P(" to "); - j = 8+15+1; - kff[CASE_P_TO_T] = ((float)readfloat(buf, &j))/10000000; - DTSTREAM.println(kff[CASE_P_TO_T],DEC); - } else - print_error(ERR("ERROR: Did not recognize FF keyword; type print k -? for more information")); - - //------- set loiterradius ------- - } else if (strmatch(buf+4, PSTR("loiterradius "))) { - loiter_radius = atoi(buf+17); - save_EEPROM_waypoint_info(); - DTSTREAM.print_P("Set loiter radius to "); DTSTREAM.print(loiter_radius,DEC); DTSTREAM.println_P(" meters"); - - //------- set rcin ------- - } else if (strmatch(buf+4, PSTR("rcin"))) { - unsigned char index = buf[8]-'1'; - if (index < 8) { - radio_in[index] = atoi(buf+9); - } else - print_error(ERR("USAGE: set rcin ")); - - //------- set rcout ------- - } else if (strmatch(buf+4, PSTR("rcout"))) { - unsigned char index = buf[9]-'1'; - if (index < 8) { - radio_out[index] = atoi(buf+10); - APM_RC.OutputCh(index, radio_out[index]); - } else - print_error(ERR("USAGE: set rcout ")); - - //------- set wpradius ------- - } else if (strmatch(buf+4, PSTR("wpradius "))) { - wp_radius = atoi(buf+13); - save_EEPROM_waypoint_info(); - DTSTREAM.print_P("Set waypoint radius to "); DTSTREAM.print(wp_radius,DEC); DTSTREAM.println_P(" meters"); - - //------- set xtrackentryangle ------- - } else if (strmatch(buf+4, PSTR("xtrackentryangle "))) { - unsigned char j = 21; - x_track_angle = readfloat(buf, &j)/100000; - DTSTREAM.print_P("Crosstrack entry angle set to "); DTSTREAM.println((float)x_track_angle/100,2); - - //------- set xtrackgain ------- - } else if (strmatch(buf+4, PSTR("xtrackgain "))) { - unsigned char j = 15; - x_track_gain = ((float)readfloat(buf, &j))/10000000; - DTSTREAM.print_P("Crosstrack gain set to "); DTSTREAM.println(x_track_gain,2); - - } else - print_error(ERR("USAGE: set {cmd|cmdcount|cmdindex|cruise|holdalt|kp|ki|kd|kff|loiterradius|rcin|rcout|wpradius}")); - - //*********** Process 'status' commands *********** - } else if (strmatch(buf, PSTR("status "))) { - //------- status control ------- - if (strmatch(buf+7, PSTR("control"))) { - DTSTREAM.println_P("Control status:"); - DTSTREAM.print_P(" Roll: nav="); DTSTREAM.print(nav_roll/100.0,2); DTSTREAM.print_P(", servo_out="); DTSTREAM.println((float)servo_out[CH_ROLL]/100.0,2); - DTSTREAM.print_P(" Pitch: nav="); DTSTREAM.print(nav_pitch/100.0,2); DTSTREAM.print_P(", servo_out="); DTSTREAM.println((float)servo_out[CH_PITCH]/100.0,2); - DTSTREAM.print_P(" Throttle: nav="); DTSTREAM.print(throttle_cruise,DEC); DTSTREAM.print_P(", servo_out="); DTSTREAM.println(servo_out[CH_THROTTLE],DEC); - - //------- status gps ------- - } else if (strmatch(buf+7, PSTR("gps"))) { - DTSTREAM.println_P("GPS status:"); - DTSTREAM.print_P(" Fix: "); - if (GPS.fix == 0) - DTSTREAM.print_P("INVALID ("); - else - DTSTREAM.print_P("Valid ("); - DTSTREAM.print(GPS.fix,DEC); - DTSTREAM.println_P(")"); - DTSTREAM.print_P(" Satellites: "); DTSTREAM.println(GPS.num_sats,DEC); - DTSTREAM.print_P(" Fix count: "); DTSTREAM.println(gps_fix_count,DEC); - - //------- status landing ------- - } else if (strmatch(buf+7, PSTR("landing"))) { - DTSTREAM.println_P("Landing status:"); - DTSTREAM.print_P(" land_complete = "); DTSTREAM.println(land_complete,DEC); - DTSTREAM.print_P(" landing_pitch = "); DTSTREAM.println(landing_pitch,DEC); - DTSTREAM.print_P(" nav_pitch = "); DTSTREAM.println(nav_pitch,DEC); - DTSTREAM.print_P(" airspeed_cruise = "); DTSTREAM.println(airspeed_cruise,DEC); - DTSTREAM.print_P(" throttle_cruise = "); DTSTREAM.println(throttle_cruise,DEC); - DTSTREAM.print_P(" hold_course = "); DTSTREAM.println(hold_course,DEC); - DTSTREAM.print_P(" nav_bearing = "); DTSTREAM.println(nav_bearing,DEC); - DTSTREAM.print_P(" bearing_error = "); DTSTREAM.println(bearing_error,DEC); - - //------- status loiter ------- - } else if (strmatch(buf+7, PSTR("loiter"))) { - DTSTREAM.println_P("Loiter status:"); - DTSTREAM.print_P(" Loiter radius: "); DTSTREAM.println(loiter_radius,DEC); - DTSTREAM.print_P(" Progress: "); DTSTREAM.print(loiter_sum,DEC); DTSTREAM.print_P("°/"); DTSTREAM.print(loiter_total,DEC); DTSTREAM.println_P("°"); - DTSTREAM.print_P(" Time: "); DTSTREAM.print(loiter_time,DEC); DTSTREAM.print_P("ms/"); DTSTREAM.print(loiter_time_max,DEC); DTSTREAM.println_P("ms"); - - //------- status navigation ------- - } else if (strmatch(buf+7, PSTR("navigation"))) { - DTSTREAM.println_P("Navigation status:"); - DTSTREAM.print_P(" From <"); DTSTREAM.print((float)prev_WP.lat/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)prev_WP.lng/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)prev_WP.alt/100.0,1); DTSTREAM.print_P(">: "); print_rlocation(&prev_WP); - DTSTREAM.print_P(" At <"); DTSTREAM.print((float)current_loc.lat/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)current_loc.lng/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)current_loc.alt/100.0,1); DTSTREAM.println_P(">"); - DTSTREAM.print_P(" To <"); DTSTREAM.print((float)next_WP.lat/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)next_WP.lng/10000000.0,6); DTSTREAM.print_P(", "); DTSTREAM.print((float)next_WP.alt/100.0,1); DTSTREAM.print_P(">: "); print_rlocation(&next_WP); - DTSTREAM.print_P(" Distance: "); DTSTREAM.print(100.0*(float)(wp_totalDistance-wp_distance)/(float)wp_totalDistance,1); DTSTREAM.print_P("% "); DTSTREAM.print(wp_totalDistance-wp_distance,DEC); DTSTREAM.print_P("m / "); DTSTREAM.print(wp_totalDistance,DEC); DTSTREAM.print_P("m; "); DTSTREAM.print(altitude_error/100.0,2); DTSTREAM.println_P("m vertically"); - DTSTREAM.print_P(" Nav bearing: "); DTSTREAM.print(nav_bearing/100.0,2); DTSTREAM.print_P("; error = "); DTSTREAM.println(bearing_error/100.0,2); - DTSTREAM.print_P(" Ground course: "); DTSTREAM.print(GPS.ground_course/100.0,1); DTSTREAM.print_P(" (current), "); DTSTREAM.print(target_bearing/100.0,1); DTSTREAM.println_P(" (target)"); - if (hold_course >= 0) { - DTSTREAM.print_P(" HOLD COURSE: "); DTSTREAM.println(hold_course/100.0,2); - } - - /*DTSTREAM.print_P(" Crosstrack bearing: "); DTSTREAM.println(crosstrack_bearing/100.0,DEC); - DTSTREAM.print_P(" Crosstrack error: "); DTSTREAM.println(crosstrack_error/100.0,DEC); - DTSTREAM.print_P(" Climb rate: "); DTSTREAM.println(climb_rate/100.0,DEC);*/ - - - //------- status navpitch ------- - } else if (strmatch(buf+7, PSTR("navpitch"))) { -#if AIRSPEED_SENSOR == 1 - DTSTREAM.print_P("nav_pitch = PID[airspeed_error ("); DTSTREAM.print(airspeed_error/100.0,2); DTSTREAM.print_P(") = airspeed_cruise ("); DTSTREAM.print((float)airspeed_cruise/100.0,2); DTSTREAM.print_P(") - airspeed ("); DTSTREAM.print((float)airspeed/100.0,2); DTSTREAM.println_P(")]"); -#else - DTSTREAM.print_P("nav_pitch = PID[altitude_error ("); DTSTREAM.print(altitude_error,DEC); DTSTREAM.print_P(") = target_altitude ("); DTSTREAM.print(target_altitude,DEC); DTSTREAM.print_P(") - current_loc.alt ("); DTSTREAM.print(current_loc.alt,DEC); DTSTREAM.println_P(")]"); -#endif - DTSTREAM.print_P("nav_pitch ("); DTSTREAM.print((float)nav_pitch/100.0,2); DTSTREAM.print_P(") - pitch_sensor ("); DTSTREAM.print((float)pitch_sensor/100.0,2); DTSTREAM.print_P(") + pitch_comp ("); DTSTREAM.print(abs(roll_sensor*kff[CASE_PITCH_COMP])/100.0,2); DTSTREAM.print_P(") = "); DTSTREAM.println((float)(nav_pitch-pitch_sensor+abs(roll_sensor*kff[CASE_PITCH_COMP]))/100.0,2); - DTSTREAM.print_P("servo_out[CH_PITCH] ("); DTSTREAM.print((float)servo_out[CH_PITCH]/100.0,2); DTSTREAM.println_P(") = PID[nav_pitch + pitch_comp - pitch_sensor]"); - - //------- status navroll ------- - } else if (strmatch(buf+7, PSTR("navroll"))) { - print_rlocation(&next_WP); - DTSTREAM.print_P("bearing_error ("); DTSTREAM.print(bearing_error,DEC); DTSTREAM.print_P(") = nav_bearing ("); DTSTREAM.print(nav_bearing,DEC); DTSTREAM.print_P(") - yaw_sensor ("); DTSTREAM.print(yaw_sensor,DEC); DTSTREAM.println_P(")"); - DTSTREAM.print_P("nav_roll ("); DTSTREAM.print(nav_roll,DEC); DTSTREAM.print_P(") - roll_sensor ("); DTSTREAM.print(roll_sensor,DEC); DTSTREAM.print_P(") = "); DTSTREAM.print(nav_roll-roll_sensor,DEC); DTSTREAM.println_P(")"); - DTSTREAM.print_P("servo_out[CH_ROLL] = "); DTSTREAM.println(servo_out[CH_ROLL],DEC); - - //------- status rcinputch ------- - } else if (strmatch(buf+7, PSTR("rcinputch"))) { - unsigned char i; - DTSTREAM.println_P("RC hardware input:"); - for (i=0; i<8; i++) { - DTSTREAM.print_P(" Ch"); - DTSTREAM.print(i+1,DEC); - DTSTREAM.print_P(": "); - DTSTREAM.println(APM_RC.InputCh(i)); - } - - //------- status rcin ------- - } else if (strmatch(buf+7, PSTR("rcin"))) { - unsigned char i; - DTSTREAM.println_P("RC software input:"); - for (i=0; i<8; i++) { - DTSTREAM.print_P(" Ch"); - DTSTREAM.print(i+1,DEC); - DTSTREAM.print_P(": "); - DTSTREAM.println(radio_in[i]); - } - - //------- status rcout ------- - } else if (strmatch(buf+7, PSTR("rcout"))) { - unsigned char i; - DTSTREAM.println_P("RC software output:"); - for (i=0; i<8; i++) { - DTSTREAM.print_P(" Ch"); - DTSTREAM.print(i+1,DEC); - DTSTREAM.print_P(": "); - DTSTREAM.println(radio_out[i]); - } - - //------- status rcpwm ------- - } else if (strmatch(buf+7, PSTR("rcpwm"))) { - unsigned char i; - DTSTREAM.println_P("RC hardware output:"); - for (i=0; i<8; i++) { - DTSTREAM.print_P(" Ch"); - DTSTREAM.print(i+1,DEC); - DTSTREAM.print_P(": "); - DTSTREAM.println(readOutputCh(i)); - } - - //------- status rctrim ------- - } else if (strmatch(buf+7, PSTR("rctrim"))) { - unsigned char i; - DTSTREAM.println_P("RC trim:"); - for (i=0; i<8; i++) { - DTSTREAM.print_P(" Ch"); - DTSTREAM.print(i+1,DEC); - DTSTREAM.print_P(": "); - DTSTREAM.println(radio_trim[i]); - } - DTSTREAM.print_P(" elevon1_trim = "); DTSTREAM.println(elevon1_trim,DEC); - DTSTREAM.print_P(" elevon2_trim = "); DTSTREAM.println(elevon2_trim,DEC); - - //------- status rc ------- - } else if (strmatch(buf+7, PSTR("rc"))) { - unsigned char i; - DTSTREAM.println_P("RC:\tCh\tHWin\tSWtrim\tSWin\tServo\tSWout\tHWout\t"); - for (i=0; i<8; i++) { - DTSTREAM.print_P("\t"); DTSTREAM.print(i+1,DEC); - DTSTREAM.print_P("\t"); DTSTREAM.print(APM_RC.InputCh(i),DEC); - DTSTREAM.print_P("\t"); DTSTREAM.print(radio_trim[i],DEC); - DTSTREAM.print_P("\t"); DTSTREAM.print(radio_in[i],DEC); - DTSTREAM.print_P("\t"); DTSTREAM.print(((float)servo_out[i])/100,2); - DTSTREAM.print_P("\t"); DTSTREAM.print(radio_out[i],DEC); - DTSTREAM.print_P("\t"); DTSTREAM.println(readOutputCh(i),DEC); - } - - //------- status system ------- - } else if (strmatch(buf+7, PSTR("system"))) { - DTSTREAM.println_P("System status:"); - DTSTREAM.print_P(" Ground start: "); if (ground_start) DTSTREAM.print_P("YES ("); else DTSTREAM.print_P("NO ("); - DTSTREAM.print(ground_start,DEC); DTSTREAM.println_P(")"); - DTSTREAM.print_P(" Home: "); if (home_is_set) DTSTREAM.print_P(" SET("); else DTSTREAM.print_P("NOT SET ("); - DTSTREAM.print(home_is_set,DEC); DTSTREAM.println_P(")"); - - //------- status takeoff ------- - } else if (strmatch(buf+7, PSTR("takeoff"))) { - DTSTREAM.println_P("Takeoff status:"); - DTSTREAM.print_P(" takeoff_pitch = "); DTSTREAM.println((float)takeoff_pitch/100.0,2); - DTSTREAM.print_P(" scaler = "); DTSTREAM.println((float)airspeed/(float)airspeed_cruise,3); - DTSTREAM.print_P(" nav_pitch = "); DTSTREAM.println((float)nav_pitch/100.0,2); - DTSTREAM.print_P(" throttle = "); DTSTREAM.println(servo_out[CH_THROTTLE],DEC); - DTSTREAM.print_P(" hold_course = "); DTSTREAM.println((float)hold_course/100.0,2); - DTSTREAM.print_P(" nav_bearing = "); DTSTREAM.println(nav_bearing,DEC); - DTSTREAM.print_P(" bearing_error = "); DTSTREAM.println(bearing_error,DEC); - DTSTREAM.print_P(" current_loc.alt = "); DTSTREAM.println(current_loc.alt,DEC); - DTSTREAM.print_P(" home.alt + takeoff_altitude = "); DTSTREAM.println(home.alt + takeoff_altitude,DEC); - - //nav_pitch = constrain(nav_pitch, 0, takeoff_pitch * 100); - //calc_nav_pitch(); - nav_pitch = constrain(nav_pitch, 0, takeoff_pitch * 100); - - //------- status telemetry ------- - } else if (strmatch(buf+7, PSTR("telemetry"))) { - DTSTREAM.println_P("Telemetry status:"); - if (report_heartbeat) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("heartbeat"); - if (report_location) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("location"); - if (report_attitude) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("attitude"); - if (report_command) DTSTREAM.print_P(" Show "); else DTSTREAM.print_P(" Hide "); DTSTREAM.println_P("command"); - DTSTREAM.print_P(" Severity report level "); DTSTREAM.println(report_severity,DEC); - - //------- status throttle ------- - } else if (strmatch(buf+7, PSTR("throttle"))) { -#if AIRSPEED_SENSOR == 1 - DTSTREAM.print_P("airspeed_energy_error ("); DTSTREAM.print(airspeed_energy_error,DEC); DTSTREAM.print_P(") = 0.5 * (airspeed_cruise ("); DTSTREAM.print((float)airspeed_cruise/100.0,2); DTSTREAM.print_P(")^2 - airspeed ("); DTSTREAM.print((float)airspeed/100.0,2); DTSTREAM.println_P(")^2)"); - DTSTREAM.print_P("energy_error ("); DTSTREAM.print(energy_error,DEC); DTSTREAM.print_P(") = airspeed_energy_error ("); DTSTREAM.print(airspeed_energy_error,DEC); DTSTREAM.print_P(") + altitude_error*0.098 ("); DTSTREAM.print((long)((float)altitude_error*0.098),DEC); DTSTREAM.println_P(")"); - DTSTREAM.print_P("servo_out[CH_THROTTLE] ("); DTSTREAM.print(servo_out[CH_THROTTLE],DEC); DTSTREAM.println_P(") = PID[energy_error]"); -#else - DTSTREAM.print_P("altitude_error ("); DTSTREAM.print(altitude_error,DEC); DTSTREAM.print_P(") = target_altitude ("); DTSTREAM.print(target_altitude,DEC); DTSTREAM.print_P(") - current_loc.alt ("); DTSTREAM.print(current_loc.alt,DEC); DTSTREAM.println_P(")"); - DTSTREAM.print_P("servo_out[CH_THROTTLE] ("); DTSTREAM.print(servo_out[CH_THROTTLE],DEC); DTSTREAM.println_P(") = PID[altitude_error]"); -#endif - - //------- status waypoints ------- - } else if (strmatch(buf+7, PSTR("waypoints"))) { - DTSTREAM.println_P("Waypoints status:"); - DTSTREAM.print_P(" Distance: "); DTSTREAM.print(wp_distance,DEC); DTSTREAM.print_P("/"); DTSTREAM.println(wp_totalDistance,DEC); - DTSTREAM.print_P(" Index: "); DTSTREAM.print(wp_index,DEC); DTSTREAM.print_P("/"); DTSTREAM.println(wp_total,DEC); - DTSTREAM.print_P(" Next: "); DTSTREAM.println(next_wp_index,DEC); - - } else if (strmatch(buf+7, PSTR("james"))) { - DTSTREAM.println_P("James is a monkey"); - } else { - print_error(ERR("USAGE: status {control|gps|landing|loiter|mixing|navigation|navpitch|navroll|rc|rcinputch|rcin|rcout|rcpwm|rctrim|system|takeoff|telemetry|throttle|waypoints}")); - } - } else { - print_error(ERR("USAGE: {echo |groundstart|inithome|show|hide|print|status|set|reset|rtl}")); - print_error(ERR("Type -? for specific usage guidance")); - } -} - -/* strmatch compares two strings and returns 1 if they match and 0 if they don't - s2 must be stored in program memory (via PSTR) rather than RAM (like standard strings) - s1 must be at least as long as s2 for a valid match - if s1 is longer than s2, then only the beginning of s1 (the length of s2) must match s2 for a valid match */ -int strmatch(char* s1, const char* s2) { - int i = 0; - char c1 = s1[0], c2 = pgm_read_byte(s2); - - while (c1 != 0 && c2 != 0) { - if (c1 < c2) - return 0; - if (c1 > c2) - return 0; - i++; - c1 = s1[i]; - c2 = pgm_read_byte(s2+i); - } - - if (c2==0) - return 1; - else - return 0; -} - -/* readfloat parses a string written as a float starting at the offset in *i and updates *i as it parses - numbers without a decimal place are read as integers - numbers with a decimial place are multiplied by 10,000,000 and decimals beyond 7 places are discarded - parsing is terminated with either a space or a null, other characters are ignored */ -long readfloat(char* s, unsigned char* i) { - long result = 0, multiplier = 0; - char c, neg = 0; - - for (;;(*i)++) { - c = s[*i]; - if (c == ' ') - break; - else if (c == 0) - break; - else if (c == '-') - neg = 1-neg; - else if (c == '.') { - result *= 10000000; - multiplier = 1000000; - } else if (c >= '0' && c <= '9') { - if (multiplier == 0) - result = (result*10) + c-'0'; - else { - result += (c-'0')*multiplier; - multiplier /= 10; - } - } - } - - if (multiplier == 0) - result *= 10000000; - - if (neg) - return -result; - else - return result; -} - -/* process_set_cmd processing the parameters of a 'set cmd' command and takes the appropriate action - *buffer is the buffer containing the parameters of the command; it should start after the space after 'set cmd' - bufferlen is the length of the buffer; the routine will stop looking for parameters after the offset index reaches this value -*/ -#define SETPARAM_NONE (0) -#define SETPARAM_ID (1) -#define SETPARAM_P1 (2) -#define SETPARAM_LAT (3) -#define SETPARAM_LNG (4) -#define SETPARAM_ALT (5) -#define SETPARAM_P2 (6) -#define SETPARAM_P3 (7) -#define SETPARAM_P4 (8) - -void process_set_cmd(char *buffer, int bufferlen) { - unsigned char i, j, err=1, setparam=SETPARAM_NONE; - unsigned char cmdindex=0, p1=0, cmdid; - long lat, lng, alt; - Location temp; - - //Parse the command index - for (i=0; i= '0' && buffer[i] <= '9') - cmdindex = (cmdindex*10) + buffer[i]-'0'; - else - break; - - if (buffer[i] == ' ') { - //Find the end of the command-type string - i++; - for (j=i; j SETPARAM_NONE) { - //Process new parameter value - i = j+1; - lat = readfloat(buffer, &i); - temp = get_wp_with_index(cmdindex); - if (setparam == SETPARAM_ID) - temp.id = lat/10000000; - else if (setparam == SETPARAM_P1) - temp.p1 = lat/10000000; - else if (setparam == SETPARAM_ALT) - temp.alt = lat/100000; - else if (setparam == SETPARAM_LAT) - temp.lat = lat; - else if (setparam == SETPARAM_LNG) - temp.lng = lat; - else if (setparam == SETPARAM_P2) - temp.alt = lat/10000000; - else if (setparam == SETPARAM_P3) - temp.lat = lat/10000000; - else if (setparam == SETPARAM_P4) - temp.lng = lat/10000000; - cmdid = temp.id; - p1 = temp.p1; - lat = temp.lat; - lng = temp.lng; - alt = temp.alt; - err = 0; - } else { - //Process param 1 - for (i=j+1; i= '0' && buffer[i] <= '9') - p1 = (p1*10) + buffer[i]-'0'; - else - break; - } - - if (buffer[i] == ' ') { - //Process altitude - i++; - if (strmatch(buffer+i, PSTR("here"))) { - lat = GPS.latitude; - lng = GPS.longitude; - alt = get_altitude_above_home(); //GPS.altitude; - err = 0; - } else { - alt = readfloat(buffer, &i)/100000; - - if (buffer[i] == ' ') { - //Process latitude - i++; - lat = readfloat(buffer, &i); - if (strmatch(buffer+i, PSTR("here"))) { - lat = GPS.latitude; - lng = GPS.longitude; - err = 0; - } else { - if (buffer[i] == ' ') { - //Process longitude - i++; - lng = readfloat(buffer, &i); - err = 0; - } else - print_error(ERR("Error processing set cmd: Could not find longitude parameter")); - } - } else - print_error(ERR("Error processing set cmd: Could not find latitude parameter")); - } - } else - print_error(ERR("Error processing set cmd: Could not find altitude parameter")); - } - } else - print_error(ERR("Error processing set cmd: Could not find command type")); - } else - print_error(ERR("Error processing set cmd: Could not find command index")); - - if (err == 0) { - temp.id = cmdid; - if (cmdid == CMD_LAND_OPTIONS) { - temp.p1 = p1; - temp.alt = alt; - temp.lat = lat / 10000000; - temp.lng = lng / 10000000; - } else { - temp.p1 = p1; - temp.lat = lat; - temp.lng = lng; - temp.alt = alt; - } - if (cmdindex >= wp_total) { - wp_total = cmdindex+1; - save_EEPROM_waypoint_info(); - } - set_wp_with_index(temp, cmdindex); - - DTSTREAM.print_P("Set command "); - DTSTREAM.print((int)cmdindex); - DTSTREAM.print_P(" to "); - DTSTREAM.print((int)temp.id); - DTSTREAM.print_P(" with p1="); - DTSTREAM.print((int)temp.p1); - DTSTREAM.print_P(", lat="); - DTSTREAM.print(temp.lat); - DTSTREAM.print_P(", lng="); - DTSTREAM.print(temp.lng); - DTSTREAM.print_P(", alt="); - DTSTREAM.println(temp.alt); - } -} - -/* get_pid_offset_name matches a string expressed in *buffer with a pid keyword and returns the k-array - gain offset in *offset, and the length of that string expression in *length. If a good keyword - match is found, 1 is returned; 0 otherwise -*/ -int get_pid_offset_name(char *buffer, int *offset, unsigned char *length) { - if (strmatch(buffer, PSTR("servoroll"))) { - *length += 9; *offset = CASE_SERVO_ROLL; - } else if (strmatch(buffer, PSTR("servopitch"))) { - *length += 10; *offset = CASE_SERVO_PITCH; - } else if (strmatch(buffer, PSTR("servorudder"))) { - *length += 11; *offset = CASE_SERVO_RUDDER; - } else if (strmatch(buffer, PSTR("navroll"))) { - *length += 7; *offset = CASE_NAV_ROLL; - } else if (strmatch(buffer, PSTR("navpitchasp"))) { - *length += 11; *offset = CASE_NAV_PITCH_ASP; - } else if (strmatch(buffer, PSTR("navpitchalt"))) { - *length += 11; *offset = CASE_NAV_PITCH_ALT; - } else if (strmatch(buffer, PSTR("throttlete"))) { - *length += 10; *offset = CASE_TE_THROTTLE; - } else if (strmatch(buffer, PSTR("throttlealt"))) { - *length += 11; *offset = CASE_ALT_THROTTLE; - } else { - return 0; - } - - return 1; -} - -/* print_rlocation prints the relative location of the specified waypoint from the plane in easy-to-read cartesian format -*/ -void print_rlocation(Location *wp) { - float x, y; - y = (float)(wp->lat - current_loc.lat) * 0.0111194927; - x = (float)(wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927; - DTSTREAM.print_P("dP = <"); - DTSTREAM.print(abs((int)y),DEC); - if (y >= 0) DTSTREAM.print_P("N, "); else DTSTREAM.print_P("S, "); - DTSTREAM.print(abs((int)x),DEC); - if (x >= 0) DTSTREAM.print_P("E, "); else DTSTREAM.print_P("W, "); - DTSTREAM.print((float)(wp->alt - current_loc.alt)/100,1); - DTSTREAM.println_P("> meters"); -} - -/* print_error prints an error message if the user sends an invalid command -*/ -void print_error(const char *msg) { - if (msg == 0) - DTSTREAM.println_P(PSTR("ERROR: Invalid command")); - else - DTSTREAM.println_P(msg); -} - -void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 -{ - if(severity >= DEBUG_LEVEL){ - DTSTREAM.print_P("MSG: "); - DTSTREAM.println(str); - } -} - -void send_message(byte id) { - send_message(id,(long)0); -} - -void send_message(byte id, long param) { - - switch(id) { - case MSG_HEARTBEAT: - if (report_heartbeat) - print_control_mode(); - break; - - case MSG_ATTITUDE: - if (report_attitude) - print_attitude(); - break; - - case MSG_LOCATION: - if (report_location) - print_position(); - if (first_location == 0) { - send_message(0,"First location received"); - first_location = 1; - } - break; - - case MSG_COMMAND: - struct Location cmd = get_wp_with_index(param); - print_waypoint(&cmd, param); - break; - - } -} - -void pipe() -{ - DTSTREAM.print_P("|"); -} - -void print_current_waypoints() -{ - DTSTREAM.println_P("Current waypoints:"); - DTSTREAM.print_P(" Prev:"); - DTSTREAM.print_P("\t"); - DTSTREAM.print(prev_WP.lat,DEC); - DTSTREAM.print_P(",\t"); - DTSTREAM.print(prev_WP.lng,DEC); - DTSTREAM.print_P(",\t"); - DTSTREAM.println(prev_WP.alt,DEC); - - DTSTREAM.print_P(" Cur: "); - DTSTREAM.print_P("\t"); - DTSTREAM.print(current_loc.lat,DEC); - DTSTREAM.print_P(",\t"); - DTSTREAM.print(current_loc.lng,DEC); - DTSTREAM.print_P(",\t"); - DTSTREAM.println(current_loc.alt,DEC); - - DTSTREAM.print_P(" Next:"); - DTSTREAM.print(wp_index,DEC); - DTSTREAM.print_P(",\t"); - DTSTREAM.print(next_WP.lat,DEC); - DTSTREAM.print_P(",\t"); - DTSTREAM.print(next_WP.lng,DEC); - DTSTREAM.print_P(",\t"); - DTSTREAM.println(next_WP.alt,DEC); -} - -void print_position(void) -{ - DTSTREAM.print_P("Pos: "); - DTSTREAM.print(current_loc.lat,DEC); // 0 - DTSTREAM.print_P(", "); //° - DTSTREAM.print(current_loc.lng,DEC); // 1 - DTSTREAM.print_P(", "); - DTSTREAM.print(current_loc.alt,DEC); // 2 - DTSTREAM.print_P("cm, "); - DTSTREAM.print(GPS.ground_speed,DEC); // 3 - DTSTREAM.print_P("cm/s GS, "); - DTSTREAM.print(airspeed,DEC); // 4 - DTSTREAM.print_P("cm/s AS, "); - DTSTREAM.print(get_altitude_above_home(),DEC); // 5 - DTSTREAM.print_P("cm above home, "); - DTSTREAM.print(climb_rate,DEC); // 6 - DTSTREAM.print_P("? climb, "); - DTSTREAM.print(wp_distance,DEC); // 7 - DTSTREAM.print_P("m from wp, "); - DTSTREAM.print(throttle_cruise,DEC); // 8 - DTSTREAM.print_P("% throttle, "); - DTSTREAM.print(altitude_error,DEC); // 9 - DTSTREAM.println_P("alt err"); -} - -void print_attitude(void) -{ - DTSTREAM.print_P("Att: "); - DTSTREAM.print(radio_in[CH_ROLL],DEC); // 0 - DTSTREAM.print_P(" roll in, "); - DTSTREAM.print(radio_in[CH_PITCH],DEC); // 1 - DTSTREAM.print_P(" pitch in, "); - DTSTREAM.print(radio_in[CH_THROTTLE],DEC); // 2 - DTSTREAM.print_P(" throttle in, "); - DTSTREAM.print(roll_sensor,DEC); // 3 - DTSTREAM.print_P(" roll sensor, "); - DTSTREAM.print(pitch_sensor,DEC); // 4 - DTSTREAM.print_P(" pitch sensor, "); - DTSTREAM.print(GPS.ground_course,DEC); // 6 - DTSTREAM.print_P(" ground course, "); - DTSTREAM.print(target_bearing,DEC); // 7 - DTSTREAM.print_P(" target bearing, "); - DTSTREAM.print(nav_roll,DEC); // 8 - DTSTREAM.print_P(" nav roll, "); - DTSTREAM.print(loiter_sum,DEC); // 8 - DTSTREAM.print_P(" loiter sum, "); - DTSTREAM.print(servo_out[CH_ROLL],DEC); - DTSTREAM.print_P(" roll servo_out, "); - DTSTREAM.print(servo_out[CH_PITCH],DEC); - DTSTREAM.println_P(" pitch servo_out"); -} - -// required by Groundstation to plot lateral tracking course -void print_new_wp_info() -{ - DTSTREAM.print_P("New waypt ("); - DTSTREAM.print(wp_index,DEC); //0 - DTSTREAM.print_P("): "); - DTSTREAM.print(prev_WP.lat,DEC); //1 - DTSTREAM.print_P(", "); //° - DTSTREAM.print(prev_WP.lng,DEC); //2 - DTSTREAM.print_P(", "); - DTSTREAM.print(prev_WP.alt,DEC); //3 - DTSTREAM.print_P("m -> "); - DTSTREAM.print(next_WP.lat,DEC); //4 - DTSTREAM.print_P("°, "); - DTSTREAM.print(next_WP.lng,DEC); //5 - DTSTREAM.print_P("°, "); - DTSTREAM.print(next_WP.alt,DEC); //6 - DTSTREAM.print_P("m; "); - DTSTREAM.print(wp_totalDistance,DEC); //7 - DTSTREAM.print_P("m dist, "); - DTSTREAM.print(radio_trim[CH_ROLL],DEC); //8 - DTSTREAM.print_P(" roll trim, "); - DTSTREAM.print(radio_trim[CH_PITCH],DEC); //9 - DTSTREAM.println_P(" pitch trim"); -} - -void print_control_mode(void) -{ - switch (control_mode){ - case MANUAL: - DTSTREAM.println_P("##MANUAL"); - break; - case STABILIZE: - DTSTREAM.println_P("##STABILIZE"); - break; - case FLY_BY_WIRE_A: - DTSTREAM.println_P("##FBW A"); - break; - case FLY_BY_WIRE_B: - DTSTREAM.println_P("##FBW B"); - break; - case AUTO: - DTSTREAM.println_P("##AUTO"); - break; - case RTL: - DTSTREAM.println_P("##RTL"); - break; - case LOITER: - DTSTREAM.println_P("##LOITER"); - break; - case 98: - DTSTREAM.println_P("##Air Start Complete"); - break; - case 99: - DTSTREAM.println_P("##Ground Start Complete"); - break; - } -} - -void print_tuning(void) { - DTSTREAM.print_P("TUN:"); - DTSTREAM.print(servo_out[CH_ROLL]); - DTSTREAM.print_P(", "); - DTSTREAM.print(nav_roll/100,DEC); - DTSTREAM.print_P(", "); - DTSTREAM.print(roll_sensor/100,DEC); - DTSTREAM.print_P(", "); - DTSTREAM.print(servo_out[CH_PITCH]); - DTSTREAM.print_P(", "); - DTSTREAM.print(nav_pitch/100,DEC); - DTSTREAM.print_P(", "); - DTSTREAM.println(pitch_sensor/100,DEC); -} - -void printPerfData(void) -{ -} - - -void print_waypoint(struct Location *cmd,byte index) -{ - //TODO: Why does this stop printing in the middle?? --BJP - DTSTREAM.print_P(" command #: "); - DTSTREAM.print(index, DEC); - DTSTREAM.print_P(" id: "); - switch (cmd->id) { - // Command IDs - Must - case CMD_BLANK: DTSTREAM.print_P("CMD_BLANK"); break; - case CMD_WAYPOINT: DTSTREAM.print_P("CMD_WAYPOINT"); break; - case CMD_LOITER: DTSTREAM.print_P("CMD_LOITER"); break; - case CMD_LOITER_N_TURNS: DTSTREAM.print_P("CMD_LOITER_N_TURNS"); break; - case CMD_LOITER_TIME: DTSTREAM.print_P("CMD_LOITER_TIME"); break; - case CMD_RTL: DTSTREAM.print_P("CMD_RTL"); break; - case CMD_LAND: DTSTREAM.print_P("CMD_LAND"); break; - case CMD_TAKEOFF: DTSTREAM.print_P("CMD_TAKEOFF"); break; - - // Command IDs - May - case CMD_DELAY: DTSTREAM.print_P("CMD_DELAY"); break; - case CMD_CLIMB: DTSTREAM.print_P("CMD_CLIMB"); break; - case CMD_LAND_OPTIONS: DTSTREAM.print_P("CMD_LAND_OPTIONS"); break; - - // Command IDs - Now - case CMD_RESET_INDEX: DTSTREAM.print_P("CMD_RESET_INDEX"); break; - case CMD_GOTO_INDEX: DTSTREAM.print_P("CMD_GOTO_INDEX"); break; - case CMD_GETVAR_INDEX: DTSTREAM.print_P("CMD_GETVAR_INDEX"); break; - case CMD_SENDVAR_INDEX: DTSTREAM.print_P("CMD_SENDVAR_INDEX"); break; - case CMD_TELEMETRY: DTSTREAM.print_P("CMD_TELEMETRY"); break; - - case CMD_THROTTLE_CRUISE: DTSTREAM.print_P("CMD_THROTTLE_CRUISE"); break; - case CMD_AIRSPEED_CRUISE: DTSTREAM.print_P("CMD_AIRSPEED_CRUISE"); break; - case CMD_RESET_HOME: DTSTREAM.print_P("CMD_RESET_HOME"); break; - - case CMD_KP_GAIN: DTSTREAM.print_P("CMD_KP_GAIN"); break; - case CMD_KI_GAIN: DTSTREAM.print_P("CMD_KI_GAIN"); break; - case CMD_KD_GAIN: DTSTREAM.print_P("CMD_KD_GAIN"); break; - case CMD_KI_MAX: DTSTREAM.print_P("CMD_KI_MAX"); break; - case CMD_KFF_GAIN: DTSTREAM.print_P("CMD_KFF_GAIN"); break; - - case CMD_RADIO_TRIM: DTSTREAM.print_P("CMD_RADIO_TRIM"); break; - case CMD_RADIO_MAX: DTSTREAM.print_P("CMD_RADIO_MAX"); break; - case CMD_RADIO_MIN: DTSTREAM.print_P("CMD_RADIO_MIN"); break; - case CMD_ELEVON_TRIM: DTSTREAM.print_P("CMD_ELEVON_TRIM"); break; - - case CMD_INDEX: DTSTREAM.print_P("CMD_INDEX"); break; - case CMD_REPEAT: DTSTREAM.print_P("CMD_REPEAT"); break; - case CMD_RELAY: DTSTREAM.print_P("CMD_RELAY"); break; - case CMD_SERVO: DTSTREAM.print_P("CMD_SERVO"); break; - - default: DTSTREAM.print(cmd->id,DEC); - } - DTSTREAM.print_P(" p1: "); - DTSTREAM.print(cmd->p1,DEC); - DTSTREAM.print_P(" p2/alt: "); - DTSTREAM.print(cmd->alt,DEC); - DTSTREAM.print_P(" p3/lat: "); - DTSTREAM.print(cmd->lat,DEC); - DTSTREAM.print_P(" p4/lng: "); - DTSTREAM.println(cmd->lng,DEC); -} - -void print_waypoints() -{ - DTSTREAM.println_P("Commands in memory:"); - DTSTREAM.print_P(" "); - DTSTREAM.print(wp_total, DEC); - DTSTREAM.println_P(" commands total"); - // create a location struct to hold the temp Waypoints for printing - //Location tmp; - DTSTREAM.println_P(" Home: "); - struct Location cmd = get_wp_with_index(0); - print_waypoint(&cmd, 0); - DTSTREAM.println_P(" Commands: "); - - for (int i=1; i= DEBUG_LEVEL){ - Serial.print("MSG: "); - Serial.println(str); - } -} - -void print_current_waypoints() -{ -} - -void print_control_mode(void) -{ -} - -void print_attitude(void) -{ - //Serial.print("!!!VER:"); - //Serial.print(SOFTWARE_VER); //output the software version - //Serial.print(","); - -// Analogs - Serial.print("AN0:"); - Serial.print(read_adc(0)); //Reversing the sign. - Serial.print(",AN1:"); - Serial.print(read_adc(1)); - Serial.print(",AN2:"); - Serial.print(read_adc(2)); - Serial.print(",AN3:"); - Serial.print(read_adc(3)); - Serial.print (",AN4:"); - Serial.print(read_adc(4)); - Serial.print (",AN5:"); - Serial.print(read_adc(5)); - Serial.print (","); - -// DCM - Serial.print ("EX0:"); - Serial.print(convert_to_dec(DCM_Matrix[0][0])); - Serial.print (",EX1:"); - Serial.print(convert_to_dec(DCM_Matrix[0][1])); - Serial.print (",EX2:"); - Serial.print(convert_to_dec(DCM_Matrix[0][2])); - Serial.print (",EX3:"); - Serial.print(convert_to_dec(DCM_Matrix[1][0])); - Serial.print (",EX4:"); - Serial.print(convert_to_dec(DCM_Matrix[1][1])); - Serial.print (",EX5:"); - Serial.print(convert_to_dec(DCM_Matrix[1][2])); - Serial.print (",EX6:"); - Serial.print(convert_to_dec(DCM_Matrix[2][0])); - Serial.print (",EX7:"); - Serial.print(convert_to_dec(DCM_Matrix[2][1])); - Serial.print (",EX8:"); - Serial.print(convert_to_dec(DCM_Matrix[2][2])); - Serial.print (","); - -// Euler - Serial.print("RLL:"); - Serial.print(ToDeg(roll)); - Serial.print(",PCH:"); - Serial.print(ToDeg(pitch)); - Serial.print(",YAW:"); - Serial.print(ToDeg(yaw)); - Serial.print(",IMUH:"); - Serial.print(((int)imu_health>>8)&0xff); - Serial.print (","); - - - /* - #if MAGNETOMETER == ENABLED - Serial.print("MGX:"); - Serial.print(magnetom_x); - Serial.print (",MGY:"); - Serial.print(magnetom_y); - Serial.print (",MGZ:"); - Serial.print(magnetom_z); - Serial.print (",MGH:"); - Serial.print(MAG_Heading); - Serial.print (","); - #endif - */ - - //Serial.print("Temp:"); - //Serial.print(temp_unfilt/20.0); // Convert into degrees C - //alti(); - //Serial.print(",Pressure: "); - //Serial.print(press); - //Serial.print(",Alt: "); - //Serial.print(press_alt/1000); // Original floating point full solution in meters - //Serial.print (","); - Serial.println("***"); -} - -void print_location(void) -{ - Serial.print("LAT:"); - Serial.print(current_loc.lat); - Serial.print(",LON:"); - Serial.print(current_loc.lng); - Serial.print(",ALT:"); - Serial.print(current_loc.alt/100); // meters - Serial.print(",COG:"); - Serial.print(GPS.ground_course); - Serial.print(",SOG:"); - Serial.print(GPS.ground_speed); - Serial.print(",FIX:"); - Serial.print((int)GPS.fix); - Serial.print(",SAT:"); - Serial.print((int)GPS.num_sats); - Serial.print (","); - Serial.print("TOW:"); - Serial.print(GPS.time); - Serial.println("***"); -} - -void print_waypoints() -{ -} - -void print_waypoint(struct Location *cmd,byte index) -{ - Serial.print("command #: "); - Serial.print(index, DEC); - Serial.print(" id: "); - Serial.print(cmd->id,DEC); - Serial.print(" p1: "); - Serial.print(cmd->p1,DEC); - Serial.print(" p2: "); - Serial.print(cmd->alt,DEC); - Serial.print(" p3: "); - Serial.print(cmd->lat,DEC); - Serial.print(" p4: "); - Serial.println(cmd->lng,DEC); - -} - -long convert_to_dec(float x) -{ - return x*10000000; -} - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde deleted file mode 100644 index 37c9de372c..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde +++ /dev/null @@ -1,238 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - - -#if GCS_PROTOCOL == GCS_PROTOCOL_JASON - -// this is my personal GCS - Jason - - -void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 -{ - if(severity >= DEBUG_LEVEL){ - Serial.print("MSG: "); - Serial.println(str); - } -} - -void send_message(byte id) -{ - send_message(id,(long)0); -} - -void send_message(byte id, long param) -{ - - switch(id) { - case MSG_HEARTBEAT: - print_control_mode(); - break; - - case MSG_ATTITUDE: - print_attitude(); - break; - - case MSG_LOCATION: - print_position(); - break; - - case MSG_COMMAND: - struct Location cmd = get_wp_with_index(param); - print_waypoint(&cmd, param); - break; - - } -} - -void pipe() -{ - Serial.print("|"); -} - -void print_current_waypoints() -{ - Serial.print("MSG: "); - - Serial.print("CUR:"); - Serial.print("\t"); - Serial.print(current_loc.lat,DEC); - Serial.print(",\t"); - Serial.print(current_loc.lng,DEC); - Serial.print(",\t"); - Serial.println(current_loc.alt,DEC); - - Serial.print("NWP:"); - Serial.print(wp_index,DEC); - Serial.print(",\t"); - Serial.print(next_WP.lat,DEC); - Serial.print(",\t"); - Serial.print(next_WP.lng,DEC); - Serial.print(",\t"); - Serial.println(next_WP.alt,DEC); -} - -void print_position(void) -{ - Serial.print("!!"); - Serial.print(current_loc.lat,DEC); // 0 - pipe(); - Serial.print(current_loc.lng,DEC); // 1 - pipe(); - Serial.print(current_loc.alt,DEC); // 2 - pipe(); - Serial.print(GPS.ground_speed,DEC); // 3 - pipe(); - Serial.print(airspeed,DEC); // 4 - pipe(); - Serial.print(get_altitude_above_home(),DEC); // 5 - pipe(); - Serial.print(climb_rate,DEC); // 6 - pipe(); - Serial.print(wp_distance,DEC); // 7 - pipe(); - Serial.print(throttle_cruise,DEC); // 8 - pipe(); - Serial.println(altitude_error,DEC); // 9 - -} - -void print_attitude(void) -{ - //return; - - Serial.print("++"); - Serial.print(radio_in[0],DEC); // 0 - pipe(); - Serial.print(radio_in[1],DEC); // 1 - pipe(); - Serial.print(radio_in[CH_THROTTLE],DEC); // 2 - pipe(); - Serial.print(roll_sensor,DEC); // 3 - pipe(); - Serial.print(pitch_sensor,DEC); // 4 - pipe(); - Serial.print("0"); // 5 ir_max - removed, no longer applicable - pipe(); - Serial.print(yaw_sensor,DEC); // 6 - pipe(); - Serial.print(target_bearing,DEC); // 7 - pipe(); - Serial.print(nav_roll,DEC); // 8 - pipe(); - Serial.println(loiter_sum,DEC); // 8 - -} - -// required by Groundstation to plot lateral tracking course -void print_new_wp_info() -{ - Serial.print("??"); - Serial.print(wp_index,DEC); //0 - pipe(); - Serial.print(prev_WP.lat,DEC); //1 - pipe(); - Serial.print(prev_WP.lng,DEC); //2 - pipe(); - Serial.print(prev_WP.alt,DEC); //3 - pipe(); - Serial.print(next_WP.lat,DEC); //4 - pipe(); - Serial.print(next_WP.lng,DEC); //5 - pipe(); - Serial.print(next_WP.alt,DEC); //6 - pipe(); - Serial.print(wp_totalDistance,DEC); //7 - pipe(); - Serial.print(radio_trim[CH_ROLL],DEC); //8 - pipe(); - Serial.println(radio_trim[CH_PITCH],DEC); //9 -} - -void print_control_mode(void) -{ - switch (control_mode){ - case MANUAL: - Serial.println("##MANUAL"); - break; - case STABILIZE: - Serial.println("##STABILIZE"); - break; - case FLY_BY_WIRE_A: - Serial.println("##FBW A"); - break; - case FLY_BY_WIRE_B: - Serial.println("##FBW B"); - break; - case AUTO: - Serial.println("##AUTO"); - break; - case RTL: - Serial.println("##RTL"); - break; - case LOITER: - Serial.println("##LOITER"); - break; - case 98: - Serial.println("##Air Start Complete"); - break; - case 99: - Serial.println("##Ground Start Complete"); - break; - } -} - -void print_tuning(void) { - Serial.print("TUN:"); - Serial.print(servo_out[CH_ROLL]/100); - Serial.print(", "); - Serial.print(nav_roll/100,DEC); - Serial.print(", "); - Serial.print(roll_sensor/100,DEC); - Serial.print(", "); - Serial.print(servo_out[CH_PITCH]/100); - Serial.print(", "); - Serial.print(nav_pitch/100,DEC); - Serial.print(", "); - Serial.println(pitch_sensor/100,DEC); -} - -void printPerfData(void) -{ -} - - -void print_waypoint(struct Location *cmd,byte index) -{ - Serial.print("command #: "); - Serial.print(index, DEC); - Serial.print(" id: "); - Serial.print(cmd->id,DEC); - Serial.print(" p1: "); - Serial.print(cmd->p1,DEC); - Serial.print(" p2: "); - Serial.print(cmd->alt,DEC); - Serial.print(" p3: "); - Serial.print(cmd->lat,DEC); - Serial.print(" p4: "); - Serial.println(cmd->lng,DEC); -} - -void print_waypoints() -{ - Serial.println("Commands in memory"); - Serial.print("commands total: "); - Serial.println(wp_total, DEC); - // create a location struct to hold the temp Waypoints for printing - //Location tmp; - Serial.println("Home: "); - struct Location cmd = get_wp_with_index(0); - print_waypoint(&cmd, 0); - Serial.println("Commands: "); - - for (int i=1; i> 8) & 0xff; - tempint = battery_voltage1 * 100; // Battery voltage ( * 100) - mess_buffer[6] = tempint & 0xff; - mess_buffer[7] = (tempint >> 8) & 0xff; - tempint = command_must_index; // Command Index (waypoint level) - mess_buffer[8] = tempint & 0xff; - mess_buffer[9] = (tempint >> 8) & 0xff; - break; - - case MSG_ATTITUDE: // ** Attitude message - mess_buffer[0] = 0x06; - ck = 6; - tempint = roll_sensor; // Roll (degrees * 100) - mess_buffer[3] = tempint & 0xff; - mess_buffer[4] = (tempint >> 8) & 0xff; - tempint = pitch_sensor; // Pitch (degrees * 100) - mess_buffer[5] = tempint & 0xff; - mess_buffer[6] = (tempint >> 8) & 0xff; - tempint = yaw_sensor; // Yaw (degrees * 100) - mess_buffer[7] = tempint & 0xff; - mess_buffer[8] = (tempint >> 8) & 0xff; - break; - - case MSG_LOCATION: // ** Location / GPS message - mess_buffer[0] = 0x12; - ck = 18; - templong = current_loc.lat; // Latitude *10 * *7 in 4 bytes - mess_buffer[3] = templong & 0xff; - mess_buffer[4] = (templong >> 8) & 0xff; - mess_buffer[5] = (templong >> 16) & 0xff; - mess_buffer[6] = (templong >> 24) & 0xff; - - templong = current_loc.lng; // Longitude *10 * *7 in 4 bytes - mess_buffer[7] = templong & 0xff; - mess_buffer[8] = (templong >> 8) & 0xff; - mess_buffer[9] = (templong >> 16) & 0xff; - mess_buffer[10] = (templong >> 24) & 0xff; - - tempint = GPS.altitude / 100; // Altitude MSL in meters * 10 in 2 bytes - mess_buffer[11] = tempint & 0xff; - mess_buffer[12] = (tempint >> 8) & 0xff; - - tempint = GPS.ground_speed; // Speed in M / S * 100 in 2 bytes - mess_buffer[13] = tempint & 0xff; - mess_buffer[14] = (tempint >> 8) & 0xff; - - tempint = yaw_sensor; // Ground Course in degreees * 100 in 2 bytes - mess_buffer[15] = tempint & 0xff; - mess_buffer[16] = (tempint >> 8) & 0xff; - - templong = GPS.time; // Time of Week (milliseconds) in 4 bytes - mess_buffer[17] = templong & 0xff; - mess_buffer[18] = (templong >> 8) & 0xff; - mess_buffer[19] = (templong >> 16) & 0xff; - mess_buffer[20] = (templong >> 24) & 0xff; - break; - - case MSG_PRESSURE: // ** Pressure message - mess_buffer[0] = 0x04; - ck = 4; - tempint = current_loc.alt / 10; // Altitude MSL in meters * 10 in 2 bytes - mess_buffer[3] = tempint & 0xff; - mess_buffer[4] = (tempint >> 8) & 0xff; - tempint = (int)airspeed / 100; // Airspeed pressure - mess_buffer[5] = tempint & 0xff; - mess_buffer[6] = (tempint >> 8) & 0xff; - break; - -// case 0xMSG_STATUS_TEXT: // ** Status Text message -// mess_buffer[0]=sizeof(status_message[0])+1; -// ck=mess_buffer[0]; -// mess_buffer[2] = param&0xff; -// for (int i=3;i> 8) & 0xff; - mess_buffer[5] = (templong >> 16) & 0xff; - mess_buffer[6] = (templong >> 24) & 0xff; - tempint = mainLoop_count; // Main Loop cycles - mess_buffer[7] = tempint & 0xff; - mess_buffer[8] = (tempint >> 8) & 0xff; - mess_buffer[9] = G_Dt_max & 0xff; - mess_buffer[10] = gyro_sat_count; // Problem counts - mess_buffer[11] = adc_constraints; - mess_buffer[12] = renorm_sqrt_count; - mess_buffer[13] = renorm_blowup_count; - mess_buffer[14] = gps_fix_count; - tempint = (int)(imu_health * 1000); // IMU health metric - mess_buffer[15] = tempint & 0xff; - mess_buffer[16] = (tempint >> 8) & 0xff; - tempint = gcs_messages_sent; // GCS messages sent - mess_buffer[17] = tempint & 0xff; - mess_buffer[18] = (tempint >> 8) & 0xff; - break; - - case MSG_VALUE: // ** Requested Value message - mess_buffer[0] = 0x06; - ck = 6; - templong = param; // Parameter being sent - mess_buffer[3] = templong & 0xff; - mess_buffer[4] = (templong >> 8) & 0xff; - switch(param) { -// case 0x00: templong = roll_mode; break; -// case 0x01: templong = pitch_mode; break; - case 0x02: templong = yaw_mode; break; -// case 0x03: templong = throttle_mode; break; - case 0x04: templong = elevon1_trim; break; - case 0x05: templong = elevon2_trim; break; - case 0x10: templong = integrator[0] * 1000; break; - case 0x11: templong = integrator[1] * 1000; break; - case 0x12: templong = integrator[2] * 1000; break; - case 0x13: templong = integrator[3] * 1000; break; - case 0x14: templong = integrator[4] * 1000; break; - case 0x15: templong = integrator[5] * 1000; break; - case 0x16: templong = integrator[6] * 1000; break; - case 0x17: templong = integrator[7] * 1000; break; - case 0x1a: templong = kff[0]; break; - case 0x1b: templong = kff[1]; break; - case 0x1c: templong = kff[2]; break; - case 0x20: templong = target_bearing; break; - case 0x21: templong = nav_bearing; break; - case 0x22: templong = bearing_error; break; - case 0x23: templong = crosstrack_bearing; break; - case 0x24: templong = crosstrack_error; break; - case 0x25: templong = altitude_error; break; - case 0x26: templong = wp_radius; break; - case 0x27: templong = loiter_radius; break; -// case 0x28: templong = wp_mode; break; -// case 0x29: templong = loop_commands; break; - case 0x2a: templong = nav_gain_scaler; break; - } - mess_buffer[5] = templong & 0xff; - mess_buffer[6] = (templong >> 8) & 0xff; - mess_buffer[7] = (templong >> 16) & 0xff; - mess_buffer[8] = (templong >> 24) & 0xff; - break; - - case MSG_COMMAND: // Command list item message - mess_buffer[0] = 0x10; - ck = 16; - tempint = param; // item number - mess_buffer[3] = tempint & 0xff; - mess_buffer[4] = (tempint >> 8) & 0xff; - tempint = wp_total; // list length (# of commands in mission) - mess_buffer[5] = tempint & 0xff; - mess_buffer[6] = (tempint >> 8) & 0xff; - tell_command = get_wp_with_index((int)param); - mess_buffer[7] = tell_command.id; // command id - mess_buffer[8] = tell_command.p1; // P1 - tempint = tell_command.alt; // P2 - mess_buffer[9] = tempint & 0xff; - mess_buffer[10] = (tempint >> 8) & 0xff; - templong = tell_command.lat; // P3 - mess_buffer[11] = templong & 0xff; - mess_buffer[12] = (templong >> 8) & 0xff; - mess_buffer[13] = (templong >> 16) & 0xff; - mess_buffer[14] = (templong >> 24) & 0xff; - templong = tell_command.lng; // P4 - mess_buffer[15] = templong & 0xff; - mess_buffer[16] = (templong >> 8) & 0xff; - mess_buffer[17] = (templong >> 16) & 0xff; - mess_buffer[18] = (templong >> 24) & 0xff; - break; - - case MSG_TRIMS: // Radio Trims message - mess_buffer[0] = 0x10; - ck = 16; - for(int i = 0; i < 8; i++) { - tempint = radio_trim[i]; // trim values - mess_buffer[3 + 2 * i] = tempint & 0xff; - mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; - } - break; - - case MSG_MINS: // Radio Mins message - mess_buffer[0] = 0x10; - ck = 16; - for(int i = 0; i < 8; i++) { - tempint = radio_min[i]; // min values - mess_buffer[3 + 2 * i] = tempint & 0xff; - mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; - } - break; - - case MSG_MAXS: // Radio Maxs message - mess_buffer[0] = 0x10; - ck = 16; - for(int i = 0; i < 8; i++) { - tempint = radio_max[i]; // max values - mess_buffer[3 + 2 * i] = tempint & 0xff; - mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; - } - break; - - case MSG_PID: // PID Gains message - mess_buffer[0] = 0x0f; - ck = 15; - mess_buffer[3] = param & 0xff; // PID set # - templong = (kp[param] * 1000000); // P gain - mess_buffer[4] = templong & 0xff; - mess_buffer[5] = (templong >> 8) & 0xff; - mess_buffer[6] = (templong >> 16) & 0xff; - mess_buffer[7] = (templong >> 24) & 0xff; - templong = (ki[param] * 1000000); // I gain - mess_buffer[8] = templong & 0xff; - mess_buffer[9] = (templong >> 8) & 0xff; - mess_buffer[10] = (templong >> 16) & 0xff; - mess_buffer[11] = (templong >> 24) & 0xff; - templong = (kd[param] * 1000000); // D gain - mess_buffer[12] = templong & 0xff; - mess_buffer[13] = (templong >> 8) & 0xff; - mess_buffer[14] = (templong >> 16) & 0xff; - mess_buffer[15] = (templong >> 24) & 0xff; - tempint = integrator_max[param]; // Integrator max value - mess_buffer[16] = tempint & 0xff; - mess_buffer[17] = (tempint >> 8) & 0xff; - break; - } - - mess_buffer[1] = id; // Message ID - mess_buffer[2] = 0x01; // Message version - - for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]); - - for (int i = 0; i < ck + 3; i++) { - mess_ck_a += mess_buffer[i]; // Calculates checksums - mess_ck_b += mess_ck_a; - } - - SendSer(mess_ck_a); - SendSer(mess_ck_b); - - gcs_messages_sent++; -} - -void send_message(byte severity, const char *str) // This is the instance of send_message for message MSG_STATUS_TEXT -{ - if(severity >= DEBUG_LEVEL){ - byte length = strlen(str) + 1; - - byte mess_buffer[54]; - byte mess_ck_a = 0; - byte mess_ck_b = 0; - int ck; - - SendSer("4D"); // This is the message preamble - if(length > 50) length = 50; - mess_buffer[0] = length; - ck = length; - mess_buffer[1] = 0x05; // Message ID - mess_buffer[2] = 0x01; // Message version - - mess_buffer[3] = severity; - - for (int i = 3; i < ck + 2; i++) - mess_buffer[i] = str[i - 3]; // places the text into mess_buffer at locations 3+ - - for (int i = 0; i < ck + 3; i++) - SendSer(mess_buffer[i]); - - for (int i = 0; i < ck + 3; i++) { - mess_ck_a += mess_buffer[i]; // Calculates checksums - mess_ck_b += mess_ck_a; - } - SendSer(mess_ck_a); - SendSer(mess_ck_b); - } -} - -void print_current_waypoints() -{ -} - -void print_waypoint(struct Location *cmd, byte index) -{ - Serial.print("command #: "); - Serial.print(index, DEC); - Serial.print(" id: "); - Serial.print(cmd->id, DEC); - Serial.print(" p1: "); - Serial.print(cmd->p1, DEC); - Serial.print(" p2: "); - Serial.print(cmd->alt, DEC); - Serial.print(" p3: "); - Serial.print(cmd->lat, DEC); - Serial.print(" p4: "); - Serial.println(cmd->lng, DEC); -} - -void print_waypoints() -{ -} - - -#endif - -#if GCS_PROTOCOL == GCS_PROTOCOL_NONE -void acknowledge(byte id, byte check1, byte check2) {} -void send_message(byte id) {} -void send_message(byte id, long param) {} -void send_message(byte severity, const char *str) {} -void print_current_waypoints(){} -void print_waypoint(struct Location *cmd, byte index){} -void print_waypoints(){} -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde deleted file mode 100644 index 6f99c2efc1..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde +++ /dev/null @@ -1,9 +0,0 @@ -#if GCS_PROTOCOL == GCS_PROTOCOL_XDIY -void acknowledge(byte id, byte check1, byte check2) {} -void send_message(byte id) {} -void send_message(byte id, long param) {} -void send_message(byte severity, const char *str) {} -void print_current_waypoints(){} -void print_waypoint(struct Location *cmd, byte index){} -void print_waypoints(){} -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde deleted file mode 100644 index e6f4c2f235..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde +++ /dev/null @@ -1,127 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE - - -void acknowledge(byte id, byte check1, byte check2) -{ -} - -void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 -{ - if(severity >= DEBUG_LEVEL){ - Serial.print("MSG: "); - Serial.println(str); - } -} - -void send_message(byte id) { - send_message(id,0l); -} - -void send_message(byte id, long param) { - switch(id) { - case MSG_HEARTBEAT: - print_control_mode(); - break; - - case MSG_ATTITUDE: - //print_attitude(); - break; - - case MSG_LOCATION: - //print_position(); - break; - - case MSG_COMMAND: - struct Location cmd = get_wp_with_index(param); - print_waypoint(&cmd, param); - break; - - } -} - - -// required by Groundstation to plot lateral tracking course -void print_new_wp_info() -{ -} - -void print_control_mode(void) -{ - Serial.print("MSG: "); - Serial.print(flight_mode_strings[control_mode]); -} - - -void print_current_waypoints() -{ - Serial.print("MSG: "); - Serial.print("CUR:"); - Serial.print("\t"); - Serial.print(current_loc.lat,DEC); - Serial.print(",\t"); - Serial.print(current_loc.lng,DEC); - Serial.print(",\t"); - Serial.println(current_loc.alt,DEC); - - Serial.print("MSG: "); - Serial.print("NWP:"); - Serial.print(wp_index,DEC); - Serial.print(",\t"); - Serial.print(next_WP.lat,DEC); - Serial.print(",\t"); - Serial.print(next_WP.lng,DEC); - Serial.print(",\t"); - Serial.println(next_WP.alt,DEC); -} - -void print_position(void) -{ -} - -void printPerfData(void) -{ -} - -void print_attitude(void) -{ -} -void print_tuning(void) { -} -void print_waypoint(struct Location *cmd,byte index) -{ - Serial.print("MSG: command #: "); - Serial.print(index, DEC); - Serial.print(" id: "); - Serial.print(cmd->id,DEC); - Serial.print(" p1: "); - Serial.print(cmd->p1,DEC); - Serial.print(" p2: "); - Serial.print(cmd->alt,DEC); - Serial.print(" p3: "); - Serial.print(cmd->lat,DEC); - Serial.print(" p4: "); - Serial.println(cmd->lng,DEC); -} - -void print_waypoints() -{ - Serial.println("Commands in memory"); - Serial.print("commands total: "); - Serial.println(wp_total, DEC); - // create a location struct to hold the temp Waypoints for printing - //Location tmp; - Serial.println("Home: "); - struct Location cmd = get_wp_with_index(0); - print_waypoint(&cmd, 0); - Serial.println("Commands: "); - - for (int i=1; i> 8) & 0xff; -} - -void output_byte(byte value) -{ - out_buffer[buf_len++] = value; -} - -void print_buffer() -{ - byte ck_a = 0; - byte ck_b = 0; - for (byte i = 0;i < buf_len; i++){ - Serial.print (out_buffer[i]); - } - Serial.print('\r'); - Serial.print('\n'); - buf_len = 0; -} - - -#endif - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde deleted file mode 100644 index 11dfcc9b39..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde +++ /dev/null @@ -1,264 +0,0 @@ -// menu structure -// -// [status] - [data1] - [data2] - [data3] - [data4] - [dataXY] -// |_________| _______|_________|_________|_________| -// | -// [SETUP1] - [SETUP2] - [SETUPXY] -// | | | -// [para1.1] [PARAM2.1] [PARAMX.Y] -// | | | -// [para1.2] [PARAM2.2] [PARAMX.Y] -// | | | -// [para1.3] [PARAM2.3] [PARAMX.Y] - -// Statusmenu -const uint8_t mnuSTATUS = 0; -const uint8_t mnuGPS1 = 1; -const uint8_t mnuGPS2 = 2; -const uint8_t mnuIMU = 3; -const uint8_t mnuACC = 4; -const uint8_t mnuGYRO = 5; -const uint8_t mnuCOMPASS = 6; -const uint8_t mnuSERVO1 = 7; -const uint8_t mnuSERVO2 = 8; -const uint8_t mnuDATA9 = 9; - -// Setup 1 -const uint8_t mnuSETUP1 = 10; -const uint8_t mnuPARAM11 = 11; -const uint8_t mnuPARAM12 = 12; -const uint8_t mnuPARAM13 = 13; -const uint8_t mnuPARAM14 = 14; - -// Setup 2 -const uint8_t mnuSETUP2 = 20; -const uint8_t mnuPARAM21 = 21; -const uint8_t mnuPARAM22 = 22; -const uint8_t mnuPARAM23 = 23; -const uint8_t mnuPARAM24 = 24; - -// Setup 3 -const uint8_t mnuSETUP3 = 30; -const uint8_t mnuPARAM31 = 31; -const uint8_t mnuPARAM32 = 32; -const uint8_t mnuPARAM33 = 33; -const uint8_t mnuPARAM34 = 34; - -// Setup 4 -const uint8_t mnuSETUP4 = 40; -const uint8_t mnuPARAM41 = 41; -const uint8_t mnuPARAM42 = 42; -const uint8_t mnuPARAM43 = 43; -const uint8_t mnuPARAM44 = 44; - -#define LCDMaxPos 32 - -static uint8_t MnuPtr = 0; -static uint8_t statustxt[LCDMaxPos]; - -void jeti_status(const char *str) -{ - byte i; - byte length = strlen(str) + 1; - if (length > LCDMaxPos) length = LCDMaxPos; - for (i = 0; i fill up with SPACE - } - } -} - -void jeti_menuctrl(uint8_t btns) { - // Algorithm for menubuttons - // if MnuPtr == 0 then Status - // if MnuPtr <10 then Data - // if (MnuPtr mod 10) = 0 then Setupp - // if (MnuPtr mod 10) <> 0 then Parameter - - if (MnuPtr == mnuSTATUS) { // MnuPtr = Status - switch (btns) { - case JB_key_right: - MnuPtr += 1; - break; - - case JB_key_left: - MnuPtr = mnuDATA9; - break; - - case JB_key_down: - MnuPtr = mnuSETUP1; - break; - } - } - else if (MnuPtr < 10) { // MnuPtr = Data - switch (btns) { - case JB_key_right: - if (MnuPtr < mnuDATA9) MnuPtr += 1; else MnuPtr = mnuSTATUS; - break; - - case JB_key_left: - if (MnuPtr > mnuSTATUS) MnuPtr -= 1; else MnuPtr = mnuDATA9; - break; - } - } - else if (MnuPtr % 10 == 0) { // MnuPtr = Setup - switch (btns) { - case JB_key_right: - if (MnuPtr < mnuSETUP4) MnuPtr += 10; else MnuPtr = mnuSETUP1; - break; - - case JB_key_left: - if (MnuPtr > mnuSETUP1) MnuPtr -= 10; else MnuPtr = mnuSETUP4; - break; - - case JB_key_up: - MnuPtr = mnuSTATUS; - break; - - case JB_key_down: - MnuPtr += 1; - break; - } - } - else { // MnuPtr = Parameter - switch (btns) { - case JB_key_down: - if ((MnuPtr % 10) < 4) MnuPtr += 1; - break; - - case JB_key_up: - if ((MnuPtr % 10) > 0) MnuPtr -= 1; - break; - } - } -} - -void jeti_init() { - // Init Jeti Serial Port - JB.begin(); -} - -void jeti_update() { - JB.clear(0); - uint8_t Buttons = JB.readbuttons(); - jeti_menuctrl(Buttons); - switch (MnuPtr) { - case mnuGPS1: - JB.print("GPS> "); - if (GPS.fix == 1) { - JB.print("FIX"); - JB.print(" SAT>"); - JB.print((int)GPS.num_sats); - JB.setcursor(LCDLine2); - JB.print("Alt: "); - JB.print((int)GPS.altitude/100); - } else { - JB.print("no FIX"); - } - break; - - case mnuGPS2: - if (GPS.fix ==1 ) { - JB.print("Lat "); - JB.print((float)GPS.latitude/10000000, 9); - JB.setcursor(LCDLine2); - JB.print("Lon "); - JB.print((float)GPS.longitude/10000000, 9); - } else { - JB.print("no Data avail."); - } - break; - - case mnuIMU: - //read_AHRS(); - JB.print("IMU> R:"); - JB.print(roll_sensor/100,DEC); - JB.print(" P:"); - JB.print(pitch_sensor/100,DEC); - JB.setcursor(LCDLine2); - JB.print("Y:"); - JB.print(yaw_sensor/100,DEC); - break; - - case mnuACC: - for (int i = 0; i < 6; i++) { - AN[i] = APM_ADC.Ch(sensors[i]); - } - JB.print("ACC> X:"); - JB.print((int)AN[3]); - JB.setcursor(LCDLine2); - JB.print(" Y:"); - JB.print((int)AN[4]); - JB.print(" Z:"); - JB.print((int)AN[5]); - break; - - case mnuGYRO: - for (int i = 0; i < 6; i++) { - AN[i] = APM_ADC.Ch(sensors[i]); - } - JB.print("GYRO> Y:"); - JB.print((int)AN[0]); - JB.setcursor(LCDLine2); - JB.print(" R:"); - JB.print((int)AN[1]); - JB.print(" P:"); - JB.print((int)AN[2]); - break; - - case mnuCOMPASS: - JB.print("MAGN> Head:"); - JB.print((int)ToDeg(APM_Compass.Heading)); - JB.setcursor(LCDLine2); - JB.println("["); - JB.print((int)APM_Compass.Mag_X); - JB.print(comma); - JB.print((int)APM_Compass.Mag_Y); - JB.print(comma); - JB.print((int)APM_Compass.Mag_Z); - JB.println("]"); - break; - - case mnuSERVO1: - JB.print("#1:"); - JB.print((int)radio_in[CH_1]); - JB.print(" #2:"); - JB.print((int)radio_in[CH_2]); - JB.setcursor(LCDLine2); - JB.print("#3:"); - JB.print((int)radio_in[CH_3]); - JB.print(" #4:"); - JB.print((int)radio_in[CH_4]); - break; - - case mnuSERVO2: - JB.print("#Q:"); - JB.print((int)servo_out[CH_ROLL]/100); - JB.print(" #H:"); - JB.print((int)servo_out[CH_PITCH]/100); - JB.setcursor(LCDLine2); - JB.print("#G:"); - JB.print((int)servo_out[CH_THROTTLE]); - JB.print(" #S:"); - JB.print((int)servo_out[CH_RUDDER]/100); - break; - - case mnuSETUP1: - JB.print("SETUP 1"); - break; - - case mnuSTATUS: - for (byte i = 0; i<32; i++) { - JB.print(statustxt[i]); - } - //JB.print("STATUS*"); - break; - - default: - JB.print("Menu: n/a #"); - JB.print((int) MnuPtr); - break; - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde deleted file mode 100644 index 348b815b83..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde +++ /dev/null @@ -1,596 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Code to Write and Read packets from DataFlash log memory -// Code to interact with the user to dump or erase logs - -#define HEAD_BYTE1 0xA3 // Decimal 163 -#define HEAD_BYTE2 0x95 // Decimal 149 -#define END_BYTE 0xBA // Decimal 186 - - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv); -static int8_t dump_log(uint8_t argc, const Menu::arg *argv); -static int8_t erase_logs(uint8_t argc, const Menu::arg *argv); -static int8_t select_logs(uint8_t argc, const Menu::arg *argv); - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -static int8_t help_log(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("\n" - "Commands:\n" - " dump dump log \n" - " erase erase all logs\n" - " enable |all enable logging or everything\n" - " disable |all disable logging or everything\n" - "\n")); -} - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -const struct Menu::command log_menu_commands[] PROGMEM = { - {"dump", dump_log}, - {"erase", erase_logs}, - {"enable", select_logs}, - {"disable", select_logs}, - {"help", help_log} -}; - -// A Macro to create the Menu -MENU2(log_menu, "Log", log_menu_commands, print_log_menu); - -static bool -print_log_menu(void) -{ - int log_start; - int log_end; - byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); - - Serial.printf_P(PSTR("logs enabled: ")); - if (0 == 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 (log_bitmask & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) - PLOG(ATTITUDE_FAST); - PLOG(ATTITUDE_MED); - PLOG(GPS); - PLOG(PM); - PLOG(CTUN); - PLOG(NTUN); - PLOG(MODE); - PLOG(RAW); - PLOG(CMD); -#undef PLOG - } - Serial.println(); - - if (last_log_num == 0) { - Serial.printf_P(PSTR("\nNo logs available for download\n")); - } else { - - Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); - for(int i=1;i last_log_num)) { - Serial.printf_P(PSTR("bad log number\n")); - return(-1); - } - - dump_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log-1)*0x02)); - dump_log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log)*0x02))-1; - if (dump_log == last_log_num) { - dump_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); - } - Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), - dump_log, dump_log_start, dump_log_end); - Log_Read(dump_log_start, dump_log_end); - Serial.printf_P(PSTR("Log read complete\n")); -} - -static int8_t -erase_logs(uint8_t argc, const Menu::arg *argv) -{ - for(int i = 10 ; i > 0; i--) { - Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); - delay(1000); - } - Serial.printf_P(PSTR("\nErasing log...\n")); - for(int j = 1; j < 4001; j++) - DataFlash.PageErase(j); - eeprom_write_byte((uint8_t *)EE_LAST_LOG_NUM, 0); - eeprom_write_byte((uint8_t *)EE_LAST_LOG_PAGE, 1); - Serial.printf_P(PSTR("\nLog erased.\n")); -} - -static int8_t -select_logs(uint8_t argc, const Menu::arg *argv) -{ - uint16_t bits; - - if (argc != 2) { - Serial.printf_P(PSTR("missing log type\n")); - return(-1); - } - - bits = 0; - - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // that name as the argument to the command, and set the bit in - // bits accordingly. - // - if (!strcasecmp_P(argv[1].str, PSTR("all"))) { - bits = ~(bits = 0); - } else { -#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s - TARG(ATTITUDE_FAST); - TARG(ATTITUDE_MED); - TARG(GPS); - TARG(PM); - TARG(CTUN); - TARG(NTUN); - TARG(MODE); - TARG(RAW); - TARG(CMD); -#undef TARG - } - - if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { - log_bitmask |= bits; - } else { - log_bitmask &= ~bits; - } - save_user_configs(); // XXX this is a bit heavyweight... - - return(0); -} - -int8_t -process_logs(uint8_t argc, const Menu::arg *argv) -{ - log_menu.run(); -} - -// Write an attitude packet. Total length : 10 bytes -void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_ATTITUDE_MSG); - DataFlash.WriteInt(log_roll); - DataFlash.WriteInt(log_pitch); - DataFlash.WriteInt(log_yaw); - DataFlash.WriteByte(END_BYTE); -} - -// Write a performance monitoring packet. Total length : 19 bytes -void Log_Write_Performance() -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - DataFlash.WriteLong(millis()- perf_mon_timer); - DataFlash.WriteInt(mainLoop_count); - DataFlash.WriteInt(G_Dt_max); - DataFlash.WriteByte(gyro_sat_count); - DataFlash.WriteByte(adc_constraints); - DataFlash.WriteByte(renorm_sqrt_count); - DataFlash.WriteByte(renorm_blowup_count); - DataFlash.WriteByte(gps_fix_count); - DataFlash.WriteInt((int)(imu_health*1000)); - DataFlash.WriteByte(END_BYTE); -} - -// Write a command processing packet. Total length : 19 bytes -//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng) -void Log_Write_Cmd(byte num, struct Location *wp) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_CMD_MSG); - DataFlash.WriteByte(num); - DataFlash.WriteByte(wp->id); - DataFlash.WriteByte(wp->p1); - DataFlash.WriteLong(wp->alt); - DataFlash.WriteLong(wp->lat); - DataFlash.WriteLong(wp->lng); - DataFlash.WriteByte(END_BYTE); -} - -void Log_Write_Startup(byte type) -{ - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_STARTUP_MSG); - DataFlash.WriteByte(type); - DataFlash.WriteByte(wp_total); - DataFlash.WriteByte(END_BYTE); - - // create a location struct to hold the temp Waypoints for printing - struct Location cmd = get_wp_with_index(0); - Log_Write_Cmd(0, &cmd); - - for (int i=1; i7) logvar = DataFlash.ReadInt(); - else logvar = DataFlash.ReadByte(); - //if(y > 7) logvar = logvar/1000.f; - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -// Read a command processing packet -void Log_Read_Cmd() -{ - byte logvarb; - long logvarl; - - Serial.print("CMD:"); - for(int i=1;i<4;i++) { - logvarb = DataFlash.ReadByte(); - Serial.print(logvarb,DEC); - Serial.print(comma); - } - for(int i=1;i<4;i++) { - logvarl = DataFlash.ReadLong(); - Serial.print(logvarl,DEC); - Serial.print(comma); - } - Serial.println(" "); -} - -void Log_Read_Startup() -{ - byte logbyte = DataFlash.ReadByte(); - if (logbyte == TYPE_AIRSTART_MSG) - Serial.print("AIR START - "); - else if (logbyte == TYPE_GROUNDSTART_MSG) - Serial.print("GROUND START - "); - else - Serial.print("UNKNOWN STARTUP TYPE -"); - Serial.print(DataFlash.ReadByte(), DEC); - Serial.println(" commands in memory"); -} - -// Read an attitude packet -void Log_Read_Attitude() -{ - int log_roll; - int log_pitch; - uint16_t log_yaw; - - log_roll = DataFlash.ReadInt(); - log_pitch = DataFlash.ReadInt(); - log_yaw = (uint16_t)DataFlash.ReadInt(); - - Serial.print("ATT:"); - Serial.print(log_roll); - Serial.print(comma); - Serial.print(log_pitch); - Serial.print(comma); - Serial.print(log_yaw); - Serial.println(); -} - -// Read a mode packet -void Log_Read_Mode() -{ - byte mode; - - mode = DataFlash.ReadByte(); - Serial.print("MOD:"); - switch (mode) { - case 0: - Serial.println("Manual"); - break; - case 1: - Serial.println("Stab"); - break; - case 5: - Serial.println("FBW_A"); - break; - case 6: - Serial.println("FBW_B"); - break; - case 10: - Serial.println("AUTO"); - break; - case 11: - Serial.println("RTL"); - break; - case 12: - Serial.println("Loiter"); - break; - case 98: - Serial.println("AS_COM"); - break; - case 99: - Serial.println("GS_COM"); - break; - } -} - -// Read a GPS packet -void Log_Read_GPS() -{ - - Serial.print("GPS:"); - Serial.print(DataFlash.ReadLong()); // Time - Serial.print(comma); - Serial.print((int)DataFlash.ReadByte()); // Fix - Serial.print(comma); - Serial.print((int)DataFlash.ReadByte()); // Num of Sats - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/t7, 7); // Lattitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/t7, 7); // Longitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // Baro/gps altitude mix - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // GPS altitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // Ground Speed - Serial.print(comma); - Serial.println((float)DataFlash.ReadLong()/100.0); // Ground Course - -} - -// Read a raw accel/gyro packet -void Log_Read_Raw() -{ - float logvar; - Serial.print("RAW:"); - for (int y=0;y<6;y++) { - logvar = DataFlash.ReadLong()/1000.f; - Serial.print(logvar); - Serial.print(comma); - } - Serial.println(" "); -} - -// Read the DataFlash log memory : Packet Parser -void Log_Read(int start_page, int end_page) -{ - byte data; - byte log_step=0; - int packet_count=0; - int page = start_page; - - - DataFlash.StartRead(start_page); - while (page < end_page && page != -1) - { - data = DataFlash.ReadByte(); - switch(log_step) //This is a state machine to read the packets - { - case 0: - if(data==HEAD_BYTE1) // Head byte 1 - log_step++; - break; - case 1: - if(data==HEAD_BYTE2) // Head byte 2 - log_step++; - else - log_step = 0; - break; - case 2: - if(data==LOG_ATTITUDE_MSG){ - Log_Read_Attitude(); - log_step++; - - }else if(data==LOG_MODE_MSG){ - Log_Read_Mode(); - log_step++; - - }else if(data==LOG_CONTROL_TUNING_MSG){ - Log_Read_Control_Tuning(); - log_step++; - - }else if(data==LOG_NAV_TUNING_MSG){ - Log_Read_Nav_Tuning(); - log_step++; - - }else if(data==LOG_PERFORMANCE_MSG){ - Log_Read_Performance(); - log_step++; - - }else if(data==LOG_RAW_MSG){ - Log_Read_Raw(); - log_step++; - - }else if(data==LOG_CMD_MSG){ - Log_Read_Cmd(); - log_step++; - - }else if(data==LOG_STARTUP_MSG){ - Log_Read_Startup(); - log_step++; - }else { - if(data==LOG_GPS_MSG){ - Log_Read_GPS(); - log_step++; - } else { - Serial.print("Error Reading Packet: "); - Serial.print(packet_count); - log_step=0; // Restart, we have a problem... - } - } - break; - case 3: - if(data==END_BYTE){ - packet_count++; - }else{ - Serial.print("Error Reading END_BYTE "); - Serial.println(data,DEC); - } - log_step=0; // Restart sequence: new packet... - break; - } - page = DataFlash.GetPage(); - } - Serial.print("Number of packets read: "); - Serial.println(packet_count); -} - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt deleted file mode 100644 index 3e8c4edf45..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt +++ /dev/null @@ -1,75 +0,0 @@ -ArduPilotMega 1.0.0 Commands - -Command Structure in bytes -0 0x00 byte Command ID -1 0x01 byte Parameter 1 -2 0x02 int Parameter 2 -3 0x03 .. -4 0x04 long Parameter 3 -5 0x05 .. -6 0x06 .. -7 0x07 .. -8 0x08 long Parameter 4 -9 0x09 .. -10 0x0A .. -11 0x0B .. - -0x00 Reserved -.... -0x0F Reserved - -*********************************** -Commands 0x10 to 0x2F are commands that have a end criteria, eg "reached waypoint" or "reached altitude" -*********************************** -Command ID Name Parameter 1 Altitude Latitude Longitude -0x10 CMD_WAYPOINT - altitude lat lon -0x11 CMD_LOITER (indefinitely) - altitude lat lon -0x12 CMD_LOITER_N_TURNS turns altitude lat lon -0x13 CMD_LOITER_TIME time (seconds*10) altitude lat lon -0x14 CMD_RTL - altitude lat lon -0x15 CMD_LAND - altitude lat lon -0x16 CMD_TAKEOFF angle - - - - - -*********************************** -May Commands - these commands are optional to finish -Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 -0x20 CMD_DELAY - - time (milliseconds) - -0x21 CMD_CLIMB rate (cm/sec) alt (finish) - - -0x22 CMD_LAND_OPTIONS distance to WP airspeed m/s, throttle %, pitch deg - - - -*********************************** -Unexecuted commands >= 0x20 are dropped when ready for the next command <= 0x1F so plan/queue commands accordingly! -For example if you had a string of 0x21 commands following a 0x10 command that had not finished when the waypoint was -reached, the unexecuted 0x21 commands would be skipped and the next command <= 0x1F would be loaded -*********************************** -Now Commands - these commands are executed once until no more new now commands are available -0x31 CMD_RESET_INDEX -0x32 CMD_GOTO_INDEX index repeat count -0x33 CMD_GETVAR_INDEX variable ID -0x34 CMD_SENDVAR_INDEX off/on = 0/1 -0x35 CMD_TELEMETRY off/on = 0/1 - -0x40 CMD_THROTTLE_CRUISE speed -0x41 CMD_AIRSPEED_CRUISE speed -0x44 CMD_RESET_HOME altitude lat lon - -0x60 CMD_KP_GAIN array index gain value*100,000 -0x61 CMD_KI_GAIN array index gain value*100,000 -0x62 CMD_KD_GAIN array index gain value*100,000 -0x63 CMD_KI_MAX array index gain value*100,000 -0x64 CMD_KFF_GAIN array index gain value*100,000 - -0x70 CMD_RADIO_TRIM array index value -0x71 CMD_RADIO_MAX array index value -0x72 CMD_RADIO_MIN array index value -0x73 CMD_ELEVON_TRIM array index value (index 0 = elevon 1 trim, 1 = elevon 2 trim) -0x75 CMD_INDEX index - -0x80 CMD_REPEAT type value delay in sec repeat count -0x81 CMD_RELAY (0,1 to change swicth position) -0x82 CMD_SERVO number value - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde deleted file mode 100644 index 0d2a1c4ba2..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde +++ /dev/null @@ -1,228 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -void init_commands() -{ - read_EEPROM_waypoint_info(); - wp_index = 0; - command_must_index = 0; - command_may_index = 0; - next_command.id = CMD_BLANK; -} - -void update_auto() -{ - if (wp_index == wp_total){ - return_to_launch(); - //wp_index = 0; - } -} - -void reload_commands() -{ - init_commands(); - read_command_index(); // Get wp_index = command_must_index from EEPROM - if(wp_index > 0){ - decrement_WP_index; - } -} - -// Getters -// ------- -struct Location get_wp_with_index(int i) -{ - struct Location temp; - long mem; - - - // Find out proper location in memory by using the start_byte position + the index - // -------------------------------------------------------------------------------- - if (i > wp_total) { - temp.id = CMD_BLANK; - }else{ - // read WP position - mem = (WP_START_BYTE) + (i * WP_SIZE); - temp.id = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.p1 = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.alt = (long)eeprom_read_dword((uint32_t*)mem); - mem += 4; - temp.lat = (long)eeprom_read_dword((uint32_t*)mem); - mem += 4; - temp.lng = (long)eeprom_read_dword((uint32_t*)mem); - } - return temp; -} - -// Setters -// ------- -void set_wp_with_index(struct Location temp, int i) -{ - - i = constrain(i,0,wp_total); - uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - - eeprom_write_byte((uint8_t *) mem, temp.id); - - mem++; - eeprom_write_byte((uint8_t *) mem, temp.p1); - - mem++; - eeprom_write_dword((uint32_t *) mem, temp.alt); - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lat); - - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lng); -} - -void increment_WP_index() -{ - if(wp_index < wp_total){ - wp_index++; - SendDebug("MSG WP index is incremented to "); - SendDebugln(wp_index,DEC); - }else{ - SendDebug("MSG Failed to increment WP index of "); - SendDebugln(wp_index,DEC); - } -} -void decrement_WP_index() -{ - if(wp_index > 0){ - wp_index--; - } -} - -void loiter_at_location() -{ - next_WP = current_loc; -} - -// add a new command at end of command set to RTL. -void return_to_launch(void) -{ - // home is WP 0 - // ------------ - wp_index = 0; - - // Loads WP from Memory - // -------------------- - set_next_WP(&home); - - // Altitude to hold over home - // Set by configuration tool - // ------------------------- - next_WP.alt = read_alt_to_hold(); -} - -struct Location get_LOITER_home_wp() -{ - // read home position - struct Location temp = get_wp_with_index(0); - temp.id = CMD_LOITER; - - temp.alt = read_alt_to_hold(); - return temp; -} - -/* -This function stores waypoint commands -It looks to see what the next command type is and finds the last command. -*/ -void set_next_WP(struct Location *wp) -{ - //send_message(SEVERITY_LOW,"load WP"); - SendDebug("MSG wp_index: "); - SendDebugln(wp_index,DEC); - send_message(MSG_COMMAND, wp_index); - - // copy the current WP into the OldWP slot - // --------------------------------------- - prev_WP = current_loc; - // Load the next_WP slot - // --------------------- - next_WP = *wp; - // offset the altitude relative to home position - // --------------------------------------------- - next_WP.alt += home.alt; - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - target_altitude = current_loc.alt; - offset_altitude = next_WP.alt - prev_WP.alt; - - // zero out our loiter vals to watch for missed waypoints - loiter_delta = 0; - loiter_sum = 0; - loiter_total = 0; - - float rads = (abs(next_WP.lat)/t7) * 0.0174532925; - //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943 - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); - - // this is handy for the groundstation - wp_totalDistance = getDistance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - - print_current_waypoints(); - - target_bearing = get_bearing(¤t_loc, &next_WP); - old_target_bearing = target_bearing; - // this is used to offset the shrinking longitude as we go towards the poles - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); -} - - -// run this at setup on the ground -// ------------------------------- -void init_home() -{ - SendDebugln("MSG: init home"); - - // Extra read just in case - // ----------------------- - //GPS.Read(); - - // block until we get a good fix - // ----------------------------- - while (!GPS.new_data || !GPS.fix) { - GPS.update(); - } - home.id = CMD_WAYPOINT; - home.lng = GPS.longitude; // Lon * 10**7 - home.lat = GPS.latitude; // Lat * 10**7 - home.alt = GPS.altitude; - home_is_set = TRUE; - - // ground altitude in centimeters for pressure alt calculations - // ------------------------------------------------------------ - ground_alt = GPS.altitude; - press_alt = GPS.altitude; // Set initial value for filter - save_pressure_data(); - - // Save Home to EEPROM - // ------------------- - set_wp_with_index(home, 0); - - // Save prev loc - // ------------- - prev_WP = home; - - // Signal ready to fly - // Make the servos wiggle - 3 times - // ----------------------- - demo_servos(3); - send_message(SEVERITY_LOW,"\n\n Ready to FLY."); - // 12345678901234567890123456789012 - jeti_status(" X-DIY Ready to FLY"); - jeti_update(); -} - - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde deleted file mode 100644 index 93e1ae6c53..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde +++ /dev/null @@ -1,480 +0,0 @@ -// called by 10 Hz loop -// -------------------- -void update_commands(void) -{ - // This function loads commands into three buffers - // when a new command is loaded, it is processed with process_XXX() - // ---------------------------------------------------------------- - if((home_is_set == FALSE) || (control_mode != AUTO)){ - return; // don't do commands - } - - if(control_mode == AUTO){ - load_next_command(); - process_next_command(); - } - - //verify_must(); - //verify_may(); -} - - -void load_next_command() -{ - // fetch next command if it's empty - // -------------------------------- - if(next_command.id == CMD_BLANK){ - next_command = get_wp_with_index(wp_index+1); - if(next_command.id != CMD_BLANK){ - //SendDebug("MSG fetch found new cmd from list at index "); - //SendDebug((wp_index+1),DEC); - //SendDebug(" with cmd id "); - //SendDebugln(next_command.id,DEC); - } - } - - // If the preload failed, return or just Loiter - // generate a dynamic command for RTL - // -------------------------------------------- - if(next_command.id == CMD_BLANK){ - // we are out of commands! - //send_message(SEVERITY_LOW,"out of commands!"); - //SendDebug("MSG out of commands, wp_index: "); - //SendDebugln(wp_index,DEC); - - - switch (control_mode){ - case LAND: - // don't get a new command - break; - - default: - next_command = get_LOITER_home_wp(); - //SendDebug("MSG Preload RTL cmd id: "); - //SendDebugln(next_command.id,DEC); - break; - } - } -} - -void process_next_command() -{ - // these are waypoint/Must commands - // --------------------------------- - if (command_must_index == 0){ // no current command loaded - if (next_command.id >= 0x10 && next_command.id <= 0x1F ){ - increment_WP_index(); - save_command_index(); // to Recover from in air Restart - command_must_index = wp_index; - - //SendDebug("MSG new command_must_id "); - //SendDebug(next_command.id,DEC); - //SendDebug(" index:"); - //SendDebugln(command_must_index,DEC); - if (log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(wp_index, &next_command); - process_must(); - } - } - - // these are May commands - // ---------------------- - if (command_may_index == 0){ - if (next_command.id >= 0x20 && next_command.id <= 0x2F ){ - increment_WP_index();// this command is from the command list in EEPROM - command_may_index = wp_index; - //Serial.print("new command_may_index "); - //Serial.println(command_may_index,DEC); - if (log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(wp_index, &next_command); - process_may(); - } - } - - // these are do it now commands - // --------------------------- - if (next_command.id >= 0x30){ - increment_WP_index();// this command is from the command list in EEPROM - if (log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(wp_index, &next_command); - process_now(); - } - -} - -/* -These functions implement the waypoint commands. -*/ -void process_must() -{ - //SendDebug("process must index: "); - //SendDebugln(command_must_index,DEC); - - send_message(SEVERITY_LOW,"New cmd: "); - send_message(MSG_COMMAND, wp_index); - - // clear May indexes - command_may_index = 0; - command_may_ID = 0; - - command_must_ID = next_command.id; - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = 0; - - // reset navigation integrators - // ------------------------- - reset_I(); - - // loads the waypoint into Next_WP struct - // -------------------------------------- - set_next_WP(&next_command); - - switch(command_must_ID){ - - case CMD_TAKEOFF: // TAKEOFF! - // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - takeoff_pitch = next_command.p1 * 100; - takeoff_altitude = next_WP.alt; // next_WP.alt is calculated by the set_next_WP command - 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 - break; - - case CMD_WAYPOINT: // Navigate to Waypoint - break; - - case CMD_LAND: // LAND to Waypoint - break; - - case CMD_LOITER: // Loiter indefinitely - break; - - case CMD_LOITER_N_TURNS: // Loiter N Times - loiter_total = next_command.p1 * 360; - break; - - case CMD_LOITER_TIME: - loiter_time = millis(); - loiter_time_max = next_command.p1 * 10000; // seconds * 10 * 1000 - break; - - case CMD_RTL: - break; - } -} - -void process_may() -{ - //Serial.print("process_may cmd# "); - //Serial.println(wp_index,DEC); - command_may_ID = next_command.id; - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = 0; - - send_message(SEVERITY_LOW," New may command loaded:"); - send_message(MSG_COMMAND, wp_index); - - // do the command - // -------------- - switch(command_may_ID){ - - case CMD_DELAY: // Navigate to Waypoint - delay_start = millis(); - delay_timeout = next_command.lat; - break; - - case CMD_LAND_OPTIONS: // Land this puppy - //send_message(SEVERITY_LOW,"Landing"); - - // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - landing_pitch = next_command.lng * 100; - airspeed_cruise = next_command.alt * 100; - throttle_cruise = next_command.lat; - landing_distance = next_command.p1; - //landing_roll = command.lng; - - SendDebug("MSG: throttle_cruise = "); - SendDebugln(throttle_cruise,DEC); - break; - - default: - break; - } -} - -void process_now() -{ - const float t5 = 100000.0; - //Serial.print("process_now cmd# "); - //Serial.println(wp_index,DEC); - - byte id = next_command.id; - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = 0; - - send_message(SEVERITY_LOW, " New now command loaded: "); - send_message(MSG_COMMAND, wp_index); - - // do the command - // -------------- - switch(id){ - - //case CMD_AP_MODE: - //next_command.p1 = constrain(next_command.p1, MANUAL, LAND); - //set_mode(next_command.p1); - //break; - - case CMD_RESET_INDEX: - init_commands(); - break; - - case CMD_GETVAR_INDEX: - // - break; - - case CMD_SENDVAR_INDEX: - // - break; - - case CMD_TELEMETRY: - // - break; - - case CMD_AIRSPEED_CRUISE: - airspeed_cruise = next_command.p1 * 100; - // todo save to EEPROM - break; - - case CMD_THROTTLE_CRUISE: - throttle_cruise = next_command.p1; - // todo save to EEPROM - break; - - case CMD_RESET_HOME: - init_home(); - break; - - case CMD_KP_GAIN: - kp[next_command.p1] = next_command.alt/t5; - // todo save to EEPROM - break; - case CMD_KI_GAIN: - ki[next_command.p1] = next_command.alt/t5; - // todo save to EEPROM - break; - case CMD_KD_GAIN: - kd[next_command.p1] = next_command.alt/t5; - // todo save to EEPROM - break; - - case CMD_KI_MAX: - integrator_max[next_command.p1] = next_command.alt/t5; - // todo save to EEPROM - break; - - case CMD_KFF_GAIN: - kff[next_command.p1] = next_command.alt/t5; - // todo save to EEPROM - break; - - case CMD_RADIO_TRIM: - radio_trim[next_command.p1] = next_command.alt; - save_EEPROM_trims(); - break; - - case CMD_RADIO_MAX: - radio_max[next_command.p1] = next_command.alt; - save_EEPROM_radio_minmax(); - break; - - case CMD_RADIO_MIN: - radio_min[next_command.p1] = next_command.alt; - save_EEPROM_radio_minmax(); - break; - - case CMD_REPEAT: - new_event(&next_command); - break; - - case CMD_SERVO: - //Serial.print("CMD_SERVO "); - //Serial.print(next_command.p1,DEC); - //Serial.print(" PWM: "); - //Serial.println(next_command.alt,DEC); - APM_RC.OutputCh(next_command.p1, next_command.alt); - break; - - case CMD_INDEX: - command_must_index = 0; - command_may_index = 0; - wp_index = next_command.p1 - 1; - break; - - case CMD_RELAY: - if(next_command.p1 = 0){ - relay_A(); - }else{ - relay_B(); - } - break; - } -} - -// Verify commands -// --------------- -void verify_must() -{ - float power; - - switch(command_must_ID) { - - case CMD_TAKEOFF: // Takeoff! - //Serial.print("verify_must cmd# "); - //Serial.println(command_must_index,DEC); - - if (GPS.ground_speed > 3){ - if(hold_course == -1){ - // save our current course to land - hold_course = yaw_sensor; - } - } - if(hold_course > -1){ - // recalc bearing error with hold_course; - nav_bearing = hold_course; - // recalc bearing error - calc_bearing_error(); - } - if (current_loc.alt > (home.alt + takeoff_altitude)) { - command_must_index = 0; - hold_course = -1; - } - break; - - - - case CMD_LAND: - // we don't verify landing - we never go to a new Must command after Land. - if ( ((wp_distance > 0) && (wp_distance <= (2*GPS.ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300) ) - { - land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude - if(hold_course == -1){ - // save our current course to land - //hold_course = yaw_sensor; - // save the course line of the runway to land - hold_course = crosstrack_bearing; - } - } - if(hold_course > -1){ - // recalc bearing error with hold_course; - nav_bearing = hold_course; - // recalc bearing error - calc_bearing_error(); - } - update_crosstrack(); - - break; - - case CMD_WAYPOINT: // reach a waypoint - hold_course == -1; - update_crosstrack(); - if ((wp_distance > 0) && (wp_distance <= wp_radius)) { - SendDebug("MSG REACHED_WAYPOINT #"); - SendDebugln(command_must_index,DEC); - // clear the command queue; - command_must_index = 0; - } - // add in a more complex case - // Doug to do - if(loiter_sum > 300){ - send_message(SEVERITY_MEDIUM," Missed WP"); - command_must_index = 0; - } - break; - - case CMD_LOITER_N_TURNS: // LOITER N times - if (wp_distance <= loiter_radius){ - nav_bearing -= 9000; - - }else if (wp_distance < (loiter_radius + LOITER_RANGE)){ - power = -((float)(wp_distance - loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0, 1); - nav_bearing -= power * 9000; - - }else{ - update_crosstrack(); - } - if(loiter_sum > loiter_total) { - loiter_total = 0; - send_message(SEVERITY_LOW," LOITER orbits complete "); - // clear the command queue; - command_must_index = 0; - } - // recalc bearing error - nav_bearing = wrap_360(nav_bearing); - calc_bearing_error(); - break; - - case CMD_LOITER_TIME: // loiter N milliseconds - - if (wp_distance <= loiter_radius){ - nav_bearing -= 9000; - - }else if (wp_distance < (loiter_radius + LOITER_RANGE)){ - power = -((float)(wp_distance - loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0, 1); - nav_bearing -= power * 9000; - - }else{ - update_crosstrack(); - } - - if ((millis() - loiter_time) > loiter_time_max) { - send_message(SEVERITY_LOW," LOITER time complete "); - command_must_index = 0; - } - nav_bearing = wrap_360(nav_bearing); - // recalc bearing error - calc_bearing_error(); - break; - - case CMD_RTL: - if (wp_distance <= 30) { - //Serial.println("REACHED_HOME"); - command_must_index = 0; - } - break; - - case CMD_LOITER: // Just plain LOITER - break; - - default: - send_message(SEVERITY_HIGH," No current Must commands"); - break; - } -} - -void verify_may() -{ - switch(command_may_ID) { - case CMD_DELAY: - if ((millis() - delay_start) > delay_timeout){ - command_may_index = 0; - delay_timeout = 0; - } - - case CMD_LAND_OPTIONS: - if ((wp_distance > 0) && (wp_distance <= landing_distance)) { - //Serial.print("XXX REACHED_WAYPOINT #"); - //Serial.println(command_must_index,DEC); - // clear the command queue; - command_may_index = 0; - } - break; - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/config.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/config.h deleted file mode 100644 index d3e3b0e2a4..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/config.h +++ /dev/null @@ -1,569 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// -// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING -// -// DO NOT EDIT this file to adjust your configuration. Create your own -// APM_Config.h and use APM_Config.h.example as a reference. -// -// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING -/// -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// -// Default and automatic configuration details. -// -// Notes for maintainers: -// -// - Try to keep this file organised in the same order as APM_Config.h.example -// - -#include "defines.h" - -/// -/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that -/// change in your local copy of APM_Config.h. -/// -#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT -/// -/// 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 -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_HIL -// Hardware in the loop output -// -#ifndef ENABLE_HIL -# define ENABLE_HIL DISABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// GPS_PROTOCOL -// -#ifndef GPS_PROTOCOL -# error XXX -# error XXX You must define GPS_PROTOCOL in APM_Config.h -# error XXX -#endif - -// The X-Plane GCS requires the IMU GPS configuration -#if (ENABLE_HIL == ENABLED) && (GPS_PROTOCOL != GPS_PROTOCOL_IMU) -# error Must select GPS_PROTOCOL_IMU when configuring for X-Plane or Flight Gear HIL -#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 - - -////////////////////////////////////////////////////////////////////////////// -// GCS_PROTOCOL -// GCS_PORT -// -#ifndef GCS_PROTOCOL -# define GCS_PROTOCOL GCS_PROTOCOL_STANDARD -#endif - -#ifndef GCS_PORT -# if (GCS_PROTOCOL == GCS_PROTOCOL_STANDARD) || (GCS_PROTOCOL == GCS_PROTOCOL_LEGACY) || (GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL) -# define GCS_PORT 3 -# else -# define GCS_PORT 0 -# endif -#endif - -#ifndef DEBUGTERMINAL_VERBOSE -# define DEBUGTERMINAL_VERBOSE 1 //Set this to 1 to get more detail in DEBUGTERMINAL messages, or 0 to save flash space -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Serial port speeds. -// -#ifndef SERIAL0_BAUD -# define SERIAL0_BAUD 38400 -#endif -#ifndef SERIAL3_BAUD -# define SERIAL3_BAUD 115200 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Battery monitoring -// -#ifndef BATTERY_EVENT -# define BATTERY_EVENT DISABLED -#endif -#ifndef BATTERY_TYPE -# define BATTERY_TYPE 0 -#endif -#ifndef LOW_VOLTAGE -# define LOW_VOLTAGE 11.4 -#endif -#ifndef VOLT_DIV_RATIO -# define VOLT_DIV_RATIO 3.0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// INPUT_VOLTAGE -// -#ifndef INPUT_VOLTAGE -# define INPUT_VOLTAGE 5.0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - - -////////////////////////////////////////////////////////////////////////////// -// Radio channel limits -// -// Note that these are not called out in APM_Config.h.example as there -// is currently no configured behaviour for these channels. -// -#ifndef CH5_MIN -# define CH5_MIN 1000 -#endif -#ifndef CH5_MAX -# define CH5_MAX 2000 -#endif -#ifndef CH6_MIN -# define CH6_MIN 1000 -#endif -#ifndef CH6_MAX -# define CH6_MAX 2000 -#endif -#ifndef CH7_MIN -# define CH7_MIN 1000 -#endif -#ifndef CH7_MAX -# define CH7_MAX 2000 -#endif -#ifndef CH8_MIN -# define CH8_MIN 1000 -#endif -#ifndef CH8_MAX -# define CH8_MAX 2000 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// FLIGHT_MODE -// FLIGHT_MODE_CHANNEL -// -#ifndef FLIGHT_MODE_CHANNEL -# define FLIGHT_MODE_CHANNEL 8 -#endif -#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8) -# error XXX -# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8 -# error XXX -#endif - -#if !defined(FLIGHT_MODE_1) -# define FLIGHT_MODE_1 FLY_BY_WIRE_A -#endif -#if !defined(FLIGHT_MODE_2) -# define FLIGHT_MODE_2 FLY_BY_WIRE_A -#endif -#if !defined(FLIGHT_MODE_3) -# define FLIGHT_MODE_3 STABILIZE -#endif -#if !defined(FLIGHT_MODE_4) -# define FLIGHT_MODE_4 STABILIZE -#endif -#if !defined(FLIGHT_MODE_5) -# define FLIGHT_MODE_5 MANUAL -#endif -#if !defined(FLIGHT_MODE_6) -# define FLIGHT_MODE_6 MANUAL -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_FAILSAFE -// THROTTLE_FS_VALUE -// THROTTLE_FAILSAFE_ACTION -// -#ifndef THROTTLE_FAILSAFE -# define THROTTLE_FAILSAFE DISABLED -#endif -#ifndef THROTTE_FS_VALUE -# define THROTTLE_FS_VALUE 975 -#endif -#ifndef THROTTLE_FAILSAFE_ACTION -# define THROTTLE_FAILSAFE_ACTION 2 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// AUTO_TRIM -// -#ifndef AUTO_TRIM -# define AUTO_TRIM ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_REVERSE -// -#ifndef THROTTLE_REVERSE -# define THROTTLE_REVERSE DISABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_STICK_MIXING -// -#ifndef ENABLE_STICK_MIXING -# define ENABLE_STICK_MIXING ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_OUT -// -#ifndef THROTTE_OUT -# define THROTTLE_OUT ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// STARTUP BEHAVIOUR -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// GROUND_START_DELAY -// -#ifndef GROUND_START_DELAY -# define GROUND_START_DELAY 0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// ENABLE_AIR_START -// -#ifndef ENABLE_AIR_START -# define ENABLE_AIR_START DISABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// ENABLE REVERSE_SWITCH -// -#ifndef REVERSE_SWITCH -# define REVERSE_SWITCH ENABLED -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// FLIGHT AND NAVIGATION CONTROL -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// Altitude measurement and control. -// -#ifndef AOA -# define AOA 100 // XXX still 100ths of a degree -#endif -#ifndef ALT_EST_GAIN -# define ALT_EST_GAIN 0.01 -#endif -#ifndef ALTITUDE_MIX -# define ALTITUDE_MIX 0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// AIRSPEED_CRUISE -// -#ifndef AIRSPEED_CRUISE -# define AIRSPEED_CRUISE 10 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// FLY_BY_WIRE_B airspeed control -// -#ifndef AIRSPEED_FBW_MIN -# define AIRSPEED_FBW_MIN 6 -#endif -#ifndef AIRSPEED_FBW_MAX -# define AIRSPEED_FBW_MAX 30 -#endif -#ifndef THROTTLE_ALT_P -# define THROTTLE_ALT_P 0.32 -#endif -#ifndef THROTTLE_ALT_I -# define THROTTLE_ALT_I 0.04 -#endif -#ifndef THROTTLE_ALT_D -# define THROTTLE_ALT_D 0.0 -#endif -#ifndef THROTTLE_ALT_INT_MAX -# define THROTTLE_ALT_INT_MAX 20 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Throttle control -// -#ifndef THROTTLE_MIN -# define THROTTLE_MIN 0 -#endif -#ifndef THROTTLE_CRUISE -# define THROTTLE_CRUISE 45 -#endif -#ifndef THROTTLE_MAX -# define THROTTLE_MAX 75 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -#ifndef HEAD_MAX -# define HEAD_MAX 45 -#endif -#ifndef PITCH_MAX -# define PITCH_MAX 15 -#endif -#ifndef PITCH_MIN -# define PITCH_MIN -25 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Attitude control gains -// -#ifndef SERVO_ROLL_P -# define SERVO_ROLL_P 0.4 -#endif -#ifndef SERVO_ROLL_I -# define SERVO_ROLL_I 0.0 -#endif -#ifndef SERVO_ROLL_D -# define SERVO_ROLL_D 0.0 -#endif -#ifndef SERVO_ROLL_INT_MAX -# define SERVO_ROLL_INT_MAX 5 -#endif -#ifndef ROLL_SLEW_LIMIT -# define ROLL_SLEW_LIMIT 0 -#endif -#ifndef SERVO_PITCH_P -# define SERVO_PITCH_P 0.6 -#endif -#ifndef SERVO_PITCH_I -# define SERVO_PITCH_I 0.0 -#endif -#ifndef SERVO_PITCH_D -# define SERVO_PITCH_D 0.0 -#endif -#ifndef SERVO_PITCH_INT_MAX -# define SERVO_PITCH_INT_MAX 5 -#endif -#ifndef PITCH_COMP -# define PITCH_COMP 0.2 -#endif -#ifndef SERVO_YAW_P -# define SERVO_YAW_P 0.5 -#endif -#ifndef SERVO_YAW_I -# define SERVO_YAW_I 0.0 -#endif -#ifndef SERVO_YAW_D -# define SERVO_YAW_D 0.0 -#endif -#ifndef SERVO_YAW_INT_MAX -# define SERVO_YAW_INT_MAX 5 -#endif -#ifndef RUDDER_MIX -# define RUDDER_MIX 0.5 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Navigation control gains -// -#ifndef NAV_ROLL_P -# define NAV_ROLL_P 0.7 -#endif -#ifndef NAV_ROLL_I -# define NAV_ROLL_I 0.01 -#endif -#ifndef NAV_ROLL_D -# define NAV_ROLL_D 0.02 -#endif -#ifndef NAV_ROLL_INT_MAX -# define NAV_ROLL_INT_MAX 5 -#endif -#ifndef NAV_PITCH_ASP_P -# define NAV_PITCH_ASP_P 0.65 -#endif -#ifndef NAV_PITCH_ASP_I -# define NAV_PITCH_ASP_I 0.0 -#endif -#ifndef NAV_PITCH_ASP_D -# define NAV_PITCH_ASP_D 0.0 -#endif -#ifndef NAV_PITCH_ASP_INT_MAX -# define NAV_PITCH_ASP_INT_MAX 5 -#endif -#ifndef NAV_PITCH_ALT_P -# define NAV_PITCH_ALT_P 0.65 -#endif -#ifndef NAV_PITCH_ALT_I -# define NAV_PITCH_ALT_I 0.0 -#endif -#ifndef NAV_PITCH_ALT_D -# define NAV_PITCH_ALT_D 0.0 -#endif -#ifndef NAV_PITCH_ALT_INT_MAX -# define NAV_PITCH_ALT_INT_MAX 5 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Energy/Altitude control gains -// -#ifndef THROTTLE_TE_P -# define THROTTLE_TE_P 0.50 -#endif -#ifndef THROTTLE_TE_I -# define THROTTLE_TE_I 0.0 -#endif -#ifndef THROTTLE_TE_D -# define THROTTLE_TE_D 0.0 -#endif -#ifndef THROTTLE_TE_INT_MAX -# define THROTTLE_TE_INT_MAX 20 -#endif -#ifndef THROTTLE_SLEW_LIMIT -# define THROTTLE_SLEW_LIMIT 0 -#endif -#ifndef P_TO_T -# define P_TO_T 2.5 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Crosstrack compensation -// -#ifndef XTRACK_GAIN -# define XTRACK_GAIN 0.01 -#endif -#ifndef XTRACK_ENTRY_ANGLE -# define XTRACK_ENTRY_ANGLE 30 -#endif - - -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// DEBUGGING -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////// -// DEBUG_SUBSYSTEM -// -#ifndef DEBUG_SUBSYSTEM -# define DEBUG_SUBSYSTEM 0 -#endif - - -////////////////////////////////////////////////////////////////////////////// -// DEBUG_LEVEL -// -#ifndef DEBUG_LEVEL -# define DEBUG_LEVEL SEVERITY_LOW -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Dataflash logging control -// -#ifndef LOG_ATTITUDE_FAST -# define LOG_ATTITUDE_FAST DISABLED -#endif -#ifndef LOG_ATTITUDE_MED -# define LOG_ATTITUDE_MED ENABLED -#endif -#ifndef LOG_GPS -# define LOG_GPS ENABLED -#endif -#ifndef LOG_PM -# define LOG_PM ENABLED -#endif -#ifndef LOG_CTUN -# define LOG_CTUN DISABLED -#endif -#ifndef LOG_NTUN -# define LOG_NTUN DISABLED -#endif -#ifndef LOG_MODE -# define LOG_MODE ENABLED -#endif -#ifndef LOG_RAW -# define LOG_RAW DISABLED -#endif -#ifndef LOG_CMD -# define LOG_CMD ENABLED -#endif - -#ifndef DEBUG_PORT -# define DEBUG_PORT 0 -#endif - -#if DEBUG_PORT == 0 -# define SendDebug Serial.print -# define SendDebugln Serial.println -#elif DEBUG_PORT == 1 -# define SendDebug Serial1.print -# define SendDebugln Serial1.println -#elif DEBUG_PORT == 2 -# define SendDebug Serial2.print -# define SendDebugln Serial2.println -#elif DEBUG_PORT == 3 -# define SendDebug Serial3.print -# define SendDebugln Serial3.println -#endif - -////////////////////////////////////////////////////////////////////////////// -// Navigation defaults -// -#ifndef WP_RADIUS_DEFAULT -# define WP_RADIUS_DEFAULT 20 -#endif -#ifndef LOITER_RADIUS_DEFAULT -# define LOITER_RADIUS_DEFAULT 30 -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde deleted file mode 100644 index d2d348e67f..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde +++ /dev/null @@ -1,58 +0,0 @@ -void read_control_switch() -{ - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - - set_mode(flight_modes[switchPosition]); - - oldSwitchPosition = switchPosition; - - // reset navigation integrators - // ------------------------- - reset_I(); - } -} -/* -byte readSwitch(void){ - int pulsewidth = APM_RC.InputCh(FLIGHT_MODE_CHANNEL - 1); - if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; - if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; - if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; - if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual - if (pulsewidth >= 1750) return 5; // Hardware Manual - return 0; -} -*/ -byte readSwitch(void){ - int pulsewidth = APM_RC.InputCh(FLIGHT_MODE_CHANNEL - 1); - if (pulsewidth > FLIGHT_MODE_5_BOUNDARY) return 5; - if (pulsewidth > FLIGHT_MODE_4_BOUNDARY) return 4; - if (pulsewidth > FLIGHT_MODE_3_BOUNDARY) return 3; - if (pulsewidth > FLIGHT_MODE_2_BOUNDARY) return 2; - if (pulsewidth > FLIGHT_MODE_1_BOUNDARY) return 1; - return 0; -} - -void reset_control_switch() -{ - oldSwitchPosition = 0; - read_control_switch(); - SendDebug("MSG: reset_control_switch"); - SendDebugln(oldSwitchPosition , DEC); -} - -void update_servo_switches() -{ - // up is reverse - // up is elevon - mix_mode = (PINL & 128) ? 1 : 0; - if (mix_mode == 0) { - reverse_roll = (PINE & 128) ? 1 : -1; - reverse_pitch = (PINE & 64) ? 1 : -1; - reverse_rudder = (PINL & 64) ? 1 : -1; - } else { - reverse_elevons = (PINE & 128) ? 1 : -1; - reverse_ch1_elevon = (PINE & 64) ? 1 : -1; - reverse_ch2_elevon = (PINL & 64) ? 1 : -1; - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde deleted file mode 100644 index e090a763db..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde +++ /dev/null @@ -1,70 +0,0 @@ -#if DEBUG_SUBSYSTEM == 1 -void debug_subsystem() -{ - Serial.println("GPS Subsystem, RAW OUTPUT"); - - while(1){ - if(Serial1.available() > 0) // Ok, let me see, the buffer is empty? - { - delay(60); // wait for it all - while(Serial1.available() > 0){ - byte incoming = Serial1.read(); - //Serial.print(incoming,DEC); - Serial.print(incoming, BYTE); // will output Hex values - //Serial.print(","); - } - Serial.println(" "); - } - - } -} -#endif - -#if DEBUG_SUBSYSTEM == 2 -void debug_subsystem() -{ - Serial.println("Control Switch"); - - Serial.print("Control CH "); - Serial.println(flight_mode_channel, DEC); - - while(1){ - delay(20); - byte switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition){ - Serial.printf_P(PSTR("Position %d\n"), i, switchPosition); - oldSwitchPosition = switchPosition; - } - } -} -#endif - -#if DEBUG_SUBSYSTEM == 3 -void debug_subsystem() -{ - Serial.println("DIP Switch Test"); - - while(1){ - delay(100); - update_servo_switches(); - if (mix_mode == 0) { - Serial.print("Mix:standard "); - Serial.print("\t reverse_roll: "); - Serial.print(reverse_roll, DEC); - Serial.print("\t reverse_pitch: "); - Serial.print(reverse_pitch, DEC); - Serial.print("\t reverse_rudder: "); - Serial.println(reverse_rudder, DEC); - } else { - Serial.print("Mix:elevons "); - Serial.print("\t reverse_elevons: "); - Serial.print(reverse_elevons, DEC); - Serial.print("\t reverse_ch1_elevon: "); - Serial.print(reverse_ch1_elevon, DEC); - Serial.print("\t reverse_ch2_elevon: "); - Serial.println(reverse_ch2_elevon, DEC); - } - } -} -#endif - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/defines.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/defines.h deleted file mode 100644 index 878fdadf0a..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/defines.h +++ /dev/null @@ -1,345 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Internal defines, don't edit and expect things to work -// ------------------------------------------------------- - -#define DEBUG 0 -#define LOITER_RANGE 30 // for calculating power outside of loiter radius - -// GPS baud rates -// -------------- -#define NO_GPS 38400 -#define NMEA_GPS 38400 -#define EM406_GPS 57600 -#define UBLOX_GPS 38400 -#define ARDU_IMU 38400 -#define MTK_GPS 38400 -#define SIM_GPS 38400 - -// GPS type codes - use the names, not the numbers -#define GPS_PROTOCOL_NONE -1 -#define GPS_PROTOCOL_NMEA 0 -#define GPS_PROTOCOL_SIRF 1 -#define GPS_PROTOCOL_UBLOX 2 -#define GPS_PROTOCOL_IMU 3 -#define GPS_PROTOCOL_MTK 4 - -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_ROLL 0 -#define CH_PITCH 1 -#define CH_THROTTLE 2 -#define CH_RUDDER 3 -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - -#define WP_START_BYTE 0x130 // where in memory home WP is stored + all other WP -#define WP_SIZE 14 - -// GCS enumeration -#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol -#define GCS_PROTOCOL_SPECIAL 1 // special test protocol (?) -#define GCS_PROTOCOL_LEGACY 2 // legacy ArduPilot protocol -#define GCS_PROTOCOL_XPLANE 3 // X-Plane HIL simulation -#define GCS_PROTOCOL_IMU 4 // ArdiPilot IMU output -#define GCS_PROTOCOL_JASON 5 // Jason's special secret GCS protocol -#define GCS_PROTOCOL_DEBUGTERMINAL 6 //Human-readable debug interface for use with a dumb terminal -#define GCS_PROTOCOL_XDIY 7 // X-DIY custom protocol -#define GCS_PROTOCOL_NONE -1 // No GCS output - -// PID enumeration -// --------------- -#define CASE_SERVO_ROLL 0 -#define CASE_SERVO_PITCH 1 -#define CASE_SERVO_RUDDER 2 -#define CASE_NAV_ROLL 3 -#define CASE_NAV_PITCH_ASP 4 -#define CASE_NAV_PITCH_ALT 5 -#define CASE_TE_THROTTLE 6 -#define CASE_ALT_THROTTLE 7 - -// Feedforward cases -// ---------------- -#define CASE_PITCH_COMP 0 -#define CASE_RUDDER_MIX 1 -#define CASE_P_TO_T 2 - -// Auto Pilot modes -// ---------------- -#define MANUAL 0 -#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle -#define STABILIZE 2 - -#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle -#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed - // Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0 -#define AUTO 10 -#define RTL 11 -#define LOITER 12 -#define TAKEOFF 13 -#define LAND 14 - - -// Command IDs - Must -#define CMD_BLANK 0x00 // there is no command stored in the mem location requested -#define CMD_WAYPOINT 0x10 -#define CMD_LOITER 0x11 -#define CMD_LOITER_N_TURNS 0x12 -#define CMD_LOITER_TIME 0x13 -#define CMD_RTL 0x14 -#define CMD_LAND 0x15 -#define CMD_TAKEOFF 0x16 - -// Command IDs - May -#define CMD_DELAY 0x20 -#define CMD_CLIMB 0x21 // NOT IMPLEMENTED -#define CMD_LAND_OPTIONS 0x22 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - -// Command IDs - Now -//#define CMD_AP_MODE 0x30 -#define CMD_RESET_INDEX 0x31 -#define CMD_GOTO_INDEX 0x32 // NOT IMPLEMENTED -#define CMD_GETVAR_INDEX 0x33 -#define CMD_SENDVAR_INDEX 0x34 -#define CMD_TELEMETRY 0x35 - -#define CMD_THROTTLE_CRUISE 0x40 -#define CMD_AIRSPEED_CRUISE 0x41 -#define CMD_RESET_HOME 0x44 - -#define CMD_KP_GAIN 0x60 -#define CMD_KI_GAIN 0x61 -#define CMD_KD_GAIN 0x62 -#define CMD_KI_MAX 0x63 -#define CMD_KFF_GAIN 0x64 - -#define CMD_RADIO_TRIM 0x70 -#define CMD_RADIO_MAX 0x71 -#define CMD_RADIO_MIN 0x72 -#define CMD_RADIO_MIN 0x72 -#define CMD_ELEVON_TRIM 0x73 - -#define CMD_INDEX 0x75 // sets the current Must index -#define CMD_REPEAT 0x80 -#define CMD_RELAY 0x81 -#define CMD_SERVO 0x82 // move servo N to PWM value - -//repeating events -#define NO_REPEAT 0 -#define CH_4_TOGGLE 1 -#define CH_5_TOGGLE 2 -#define CH_6_TOGGLE 3 -#define CH_7_TOGGLE 4 -#define RELAY_TOGGLE 5 -#define STOP_REPEAT 10 - -// GCS Message ID's -#define MSG_ACKNOWLEDGE 0x00 -#define MSG_HEARTBEAT 0x01 -#define MSG_ATTITUDE 0x02 -#define MSG_LOCATION 0x03 -#define MSG_PRESSURE 0x04 -#define MSG_STATUS_TEXT 0x05 -#define MSG_PERF_REPORT 0x06 -#define MSG_COMMAND 0x22 -#define MSG_VALUE 0x32 -#define MSG_PID 0x42 -#define MSG_TRIMS 0x50 -#define MSG_MINS 0x51 -#define MSG_MAXS 0x52 -#define MSG_IMU_OUT 0x53 - -#define SEVERITY_LOW 1 -#define SEVERITY_MEDIUM 2 -#define SEVERITY_HIGH 3 -#define SEVERITY_CRITICAL 4 - -// Logging parameters -#define LOG_ATTITUDE_MSG 0x01 -#define LOG_GPS_MSG 0x02 -#define LOG_MODE_MSG 0X03 -#define LOG_CONTROL_TUNING_MSG 0X04 -#define LOG_NAV_TUNING_MSG 0X05 -#define LOG_PERFORMANCE_MSG 0X06 -#define LOG_RAW_MSG 0x07 -#define LOG_CMD_MSG 0x08 -#define LOG_STARTUP_MSG 0x09 -#define TYPE_AIRSTART_MSG 0x00 -#define TYPE_GROUNDSTART_MSG 0x01 - -#define MASK_LOG_ATTITUDE_FAST 0 -#define MASK_LOG_ATTITUDE_MED 2 -#define MASK_LOG_GPS 4 -#define MASK_LOG_PM 8 -#define MASK_LOG_CTUN 16 -#define MASK_LOG_NTUN 32 -#define MASK_LOG_MODE 64 -#define MASK_LOG_RAW 128 -#define MASK_LOG_CMD 256 - -// Yaw modes -#define YAW_MODE_COORDINATE_TURNS 0 -#define YAW_MODE_HOLD_HEADING 1 -#define YAW_MODE_SLIP 2 - -// Waypoint Modes -// ---------------- -#define ABS_WP 0 -#define REL_WP 1 - -// Command Queues -// --------------- -#define COMMAND_MUST 0 -#define COMMAND_MAY 1 -#define COMMAND_NOW 2 - -// Events -// ------ -#define EVENT_WILL_REACH_WAYPOINT 1 -#define EVENT_SET_NEW_WAYPOINT_INDEX 2 -#define EVENT_LOADED_WAYPOINT 3 -#define EVENT_LOOP 4 - -//GPS_fix -#define VALID_GPS 0x00 -#define BAD_GPS 0x01 -#define FAILED_GPS 0x03 - - - -#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO - -#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor -#define BATTERY_PIN1 0 // These are the pins for the voltage dividers -#define BATTERY_PIN2 1 -#define BATTERY_PIN3 2 -#define BATTERY_PIN4 3 -#define RELAY_PIN 47 - -// Hardware Parameters -#define SLIDE_SWITCH_PIN 40 -#define PUSHBUTTON_PIN 41 - -#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C -#define B_LED_PIN 36 -#define C_LED_PIN 35 - -#define HOLD_ALT_ABOVE_HOME 8 // bitmask value - -// IMU Parameters - -#define ADC_CONSTRAINT 900 -#define TRUE 1 -#define FALSE 0 -#define ADC_WARM_CYCLES 200 -#define SPEEDFILT 400 // centimeters/second - -#define GYRO_TEMP_CH 3 // The ADC channel reading the gyro temperature - -// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step -// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 -// Tested value : 418 -#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer -#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi - -// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 -// Tested values : 0.4026, ?, 0.4192 -#define Gyro_Gain_X 0.4 //X axis Gyro gain -#define Gyro_Gain_Y 0.41 //Y axis Gyro gain -#define Gyro_Gain_Z 0.41 //Z axis Gyro gain -#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second - -#define Kp_ROLLPITCH 0.0014 // Pitch&Roll Drift Correction Proportional Gain -#define Ki_ROLLPITCH 0.0000003 // Pitch&Roll Drift Correction Integrator Gain -#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain -#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain - -/*For debugging purposes*/ -#define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data - - -#define EEPROM_MAX_ADDR 4096 - -// Radio setup -#define EE_TRIM 0x00 -#define EE_MIN 0x10 -#define EE_MAX 0x20 -#define EE_ELEVON1_TRIM 0x30 -#define EE_ELEVON2_TRIM 0x32 - -// user gains -#define EE_XTRACK_GAIN 0x34 -#define EE_XTRACK_ANGLE 0x36 -#define EE_ALT_MIX 0x3B -#define EE_HEAD_MAX 0x38 -#define EE_PITCH_MAX 0x39 -#define EE_PITCH_MIN 0x3A -#define EE_KP 0x40 -#define EE_KI 0x60 -#define EE_KD 0x80 -#define EE_IMAX 0xA0 -#define EE_KFF 0xC0 -#define EE_AN_OFFSET 0xE0 - -//mission specific -#define EE_CONFIG 0X0F8 -#define EE_WP_MODE 0x0F9 -#define EE_YAW_MODE 0x0FA // not used -#define EE_WP_TOTAL 0x0FB -#define EE_WP_INDEX 0x0FC -#define EE_WP_RADIUS 0x0FD -#define EE_LOITER_RADIUS 0x0FE -#define EE_ALT_HOLD_HOME 0x0FF - -// user configs -#define EE_AIRSPEED_CRUISE 0x103 -#define EE_AIRSPEED_RATIO 0x104 -#define EE_AIRSPEED_FBW_MIN 0x108 -#define EE_AIRSPEED_FBW_MAX 0x109 -#define EE_THROTTLE_MIN 0x10A -#define EE_THROTTLE_CRUISE 0x10B -#define EE_THROTTLE_MAX 0x10C -#define EE_THROTTLE_FAILSAFE 0x10D -#define EE_THROTTLE_FS_VALUE 0x10E -#define EE_THROTTLE_FAILSAFE_ACTION 0x110 -#define EE_FLIGHT_MODE_CHANNEL 0x112 -#define EE_AUTO_TRIM 0x113 -#define EE_LOG_BITMASK 0x114 -#define EE_REVERSE_SWITCH 0x120 -#define EE_FLIGHT_MODES 0x121 - -// sensors -#define EE_ABS_PRESS_GND 0x116 -#define EE_GND_TEMP 0x11A -#define EE_GND_ALT 0x11C -#define EE_AP_OFFSET 0x11E - -// log -#define EE_LAST_LOG_PAGE 0xE00 -#define EE_LAST_LOG_NUM 0xE02 -#define EE_LOG_1_START 0xE04 - -// bits in log_bitmask -#define LOGBIT_ATTITUDE_FAST (1<<0) -#define LOGBIT_ATTITUDE_MED (1<<1) -#define LOGBIT_GPS (1<<2) -#define LOGBIT_PM (1<<3) -#define LOGBIT_CTUN (1<<4) -#define LOGBIT_NTUN (1<<5) -#define LOGBIT_MODE (1<<6) -#define LOGBIT_RAW (1<<7) -#define LOGBIT_CMD (1<<8) - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/events.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/events.pde deleted file mode 100644 index c8f94d4f23..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/events.pde +++ /dev/null @@ -1,226 +0,0 @@ -/* - This event will be called when the failsafe changes - boolean failsafe reflects the current state -*/ -void failsafe_event() -{ - if (failsafe == true){ - - // This is how to handle a failsafe. - switch(control_mode) - { - case MANUAL: // First position - set_mode(STABILIZE); - break; - - case STABILIZE: - case FLY_BY_WIRE_A: // middle position - case FLY_BY_WIRE_B: // middle position - set_mode(RTL); - throttle_cruise = THROTTLE_CRUISE; - - case CIRCLE: // middle position - break; - - case AUTO: // middle position - case LOITER: // middle position - if (throttle_failsafe_action == 1){ - set_mode(RTL); - throttle_cruise = THROTTLE_CRUISE; - } - // 2 = Stay in AUTO and ignore failsafe - break; - - case RTL: // middle position - break; - - default: - set_mode(RTL); - throttle_cruise = THROTTLE_CRUISE; - break; - } - }else{ - reset_I(); - } -} - -void low_battery_event(void) -{ - send_message(SEVERITY_HIGH,"Low Battery!"); - set_mode(RTL); - throttle_cruise = THROTTLE_CRUISE; -} - - -/* -4 simultaneous events -int event_original - original time in seconds -int event_countdown - count down to zero -byte event_countdown - ID for what to do -byte event_countdown - how many times to loop, 0 = indefinite -byte event_value - specific information for an event like PWM value -byte counterEvent - undo the event if nescessary - -count down each one - - -new event -undo_event -*/ - -void new_event(struct Location *cmd) -{ - SendDebug("New Event Loaded "); - SendDebugln(cmd->p1,DEC); - - - if(cmd->p1 == STOP_REPEAT){ - SendDebugln("STOP repeat "); - event_id = NO_REPEAT; - event_timer = -1; - undo_event = 0; - event_value = 0; - event_delay = 0; - event_repeat = 0; // convert seconds to millis - event_undo_value = 0; - repeat_forever = 0; - }else{ - // reset the event timer - event_timer = millis(); - event_id = cmd->p1; - event_value = cmd->alt; - event_delay = cmd->lat; - event_repeat = cmd->lng; // convert seconds to millis - event_undo_value = 0; - repeat_forever = (event_repeat == 0) ? 1:0; - } - - /* - Serial.print("event_id: "); - Serial.println(event_id,DEC); - Serial.print("event_value: "); - Serial.println(event_value,DEC); - Serial.print("event_delay: "); - Serial.println(event_delay,DEC); - Serial.print("event_repeat: "); - Serial.println(event_repeat,DEC); - Serial.print("event_undo_value: "); - Serial.println(event_undo_value,DEC); - Serial.print("repeat_forever: "); - Serial.println(repeat_forever,DEC); - Serial.print("Event_timer: "); - Serial.println(event_timer,DEC); - */ - perform_event(); -} - -void perform_event() -{ - if (event_repeat > 0){ - event_repeat --; - } - switch(event_id) { - case CH_4_TOGGLE: - event_undo_value = readOutputCh(4); - - APM_RC.OutputCh(4, event_value); // send to Servos - undo_event = 2; - break; - case CH_5_TOGGLE: - event_undo_value = readOutputCh(5); - APM_RC.OutputCh(5, event_value); // send to Servos - undo_event = 2; - break; - case CH_6_TOGGLE: - event_undo_value = readOutputCh(6); - APM_RC.OutputCh(6, event_value); // send to Servos - undo_event = 2; - break; - case CH_7_TOGGLE: - event_undo_value = readOutputCh(7); - APM_RC.OutputCh(7, event_value); // send to Servos - undo_event = 2; - event_undo_value = 1; - break; - case RELAY_TOGGLE: - - event_undo_value = PORTL & B00000100 ? 1:0; - if(event_undo_value == 1){ - relay_A(); - }else{ - relay_B(); - } - SendDebug("toggle relay "); - SendDebugln(PORTL,BIN); - undo_event = 2; - break; - - } -} - -void relay_A() -{ - PORTL |= B00000100; -} - -void relay_B() -{ - PORTL ^= B00000100; -} - -void update_events() -{ - // repeating events - if(undo_event == 1){ - perform_event_undo(); - undo_event = 0; - }else if(undo_event > 1){ - undo_event --; - } - - if(event_timer == -1) - return; - - if((millis() - event_timer) > event_delay){ - perform_event(); - - if(event_repeat > 0 || repeat_forever == 1){ - event_repeat--; - event_timer = millis(); - }else{ - event_timer = -1; - } - } -} - -void perform_event_undo() -{ - switch(event_id) { - case CH_4_TOGGLE: - APM_RC.OutputCh(4, event_undo_value); // send to Servos - break; - - case CH_5_TOGGLE: - APM_RC.OutputCh(5, event_undo_value); // send to Servos - break; - - case CH_6_TOGGLE: - APM_RC.OutputCh(6, event_undo_value); // send to Servos - break; - - case CH_7_TOGGLE: - APM_RC.OutputCh(7, event_undo_value); // send to Servos - break; - - case RELAY_TOGGLE: - - if(event_undo_value == 1){ - relay_A(); - }else{ - relay_B(); - } - SendDebug("untoggle relay "); - SendDebugln(PORTL,BIN); - break; - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde deleted file mode 100644 index e7dbf3d2df..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde +++ /dev/null @@ -1,230 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -//**************************************************************** -// Function that will calculate the desired direction to fly and altitude error -//**************************************************************** -void navigate() -{ - // do not navigate with corrupt data - // --------------------------------- - if (GPS.fix == 0) - { - GPS.new_data = false; - return; - } - if(next_WP.lat == 0){ - return; - } - - // We only perform most nav computations if we have new gps data to work with - // -------------------------------------------------------------------------- - if(GPS.new_data){ - GPS.new_data = false; - - // target_bearing is where we should be heading - // -------------------------------------------- - target_bearing = get_bearing(¤t_loc, &next_WP); - - // nav_bearing will includes xtrac correction - // ------------------------------------------ - nav_bearing = target_bearing; - - // waypoint distance from plane - // ---------------------------- - wp_distance = getDistance(¤t_loc, &next_WP); - - if (wp_distance < 0){ - send_message(SEVERITY_HIGH," WP error - distance < 0"); - //Serial.println(wp_distance,DEC); - //print_current_waypoints(); - return; - } - - // check if we have missed the WP - loiter_delta = (target_bearing - old_target_bearing)/100; - - // reset the old value - old_target_bearing = target_bearing; - - // wrap values - if (loiter_delta > 180) loiter_delta -= 360; - if (loiter_delta < -180) loiter_delta += 360; - loiter_sum += abs(loiter_delta); - - calc_bearing_error(); - - // control mode specific updates to nav_bearing - update_navigation(); - } -} - -void update_navigation() -{ - // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS - // ------------------------------------------------------------------------ - - // distance and bearing calcs only - if(control_mode == AUTO){ - verify_must(); - verify_may(); - }else{ - - switch(control_mode){ - case LOITER: - float power; - if (wp_distance <= loiter_radius){ - nav_bearing -= 9000; - - }else if (wp_distance < (loiter_radius + LOITER_RANGE)){ - power = -((float)(wp_distance - loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0, 1); - nav_bearing -= power * 9000; - - }else{ - update_crosstrack(); - } - nav_bearing = wrap_360(nav_bearing); - calc_bearing_error(); - break; - - case RTL: - if(wp_distance <= (loiter_radius + 10)) { // + 10 meters - set_mode(LOITER); - }else{ - update_crosstrack(); - } - break; - } - } -} - - -/* -Disabled for now -void calc_distance_error() -{ - //distance_estimate += (float)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); -} -*/ - -void calc_airspeed_errors() -{ - airspeed_error = airspeed_cruise - airspeed; - airspeed_energy_error = (long)(((long)airspeed_cruise * (long)airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation -} - -void calc_bearing_error() -{ - bearing_error = nav_bearing - yaw_sensor; - bearing_error = wrap_180(bearing_error); -} - -void calc_altitude_error() -{ - // limit climb rates - target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30)); - if(prev_WP.alt > next_WP.alt){ - target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt); - }else{ - target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); - } - - /* - // Disabled for now - #if AIRSPEED_SENSOR == 1 - // special thanks to Ryan Beall for this one - float pitch_angle = pitch_sensor - AOA; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°) - pitch_angle = constrain(pitch_angle, -2000, 2000); - float scale = sin(radians(pitch_angle * .01)); - altitude_estimate += (float)airspeed * .0002 * scale; - altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt); - - // compute altitude error for throttle control - altitude_error = target_altitude - altitude_estimate; - #else - altitude_error = target_altitude - current_loc.alt; - #endif - */ - - altitude_error = target_altitude - current_loc.alt; -} - - -long wrap_360(long error) -{ - if (error > 36000) error -= 36000; - if (error < 0) error += 36000; - return error; -} - -long wrap_180(long error) -{ - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; -} - -/* -// disabled for now -void update_loiter() -{ - loiter_delta = (target_bearing - old_target_bearing)/100; - // reset the old value - old_target_bearing = target_bearing; - // wrap values - if (loiter_delta > 170) loiter_delta -= 360; - if (loiter_delta < -170) loiter_delta += 360; - loiter_sum += loiter_delta; -}*/ - -void update_crosstrack(void) -{ - // Crosstrack Error - // ---------------- - if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing)/100)) * wp_distance; // Meters we are off track line - nav_bearing += constrain(crosstrack_error * x_track_gain, -x_track_angle, x_track_angle); - nav_bearing = wrap_360(nav_bearing); - } -} - -void reset_crosstrack() -{ - crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following -} - -int get_altitude_above_home(void) -{ - // This is the altitude above the home location - // The GPS gives us altitude at Sea Level - // if you slope soar, you should see a negative number sometimes - // ------------------------------------------------------------- - return current_loc.alt - home.alt; -} - -long getDistance(struct Location *loc1, struct Location *loc2) -{ - if(loc1->lat == 0 || loc1->lng == 0) - return -1; - if(loc2->lat == 0 || loc2->lng == 0) - return -1; - float dlat = (float)(loc2->lat - loc1->lat); - float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; - return sqrt(sq(dlat) + sq(dlong)) * .01113195; -} - -long get_alt_distance(struct Location *loc1, struct Location *loc2) -{ - return abs(loc1->alt - loc2->alt); -} - -long get_bearing(struct Location *loc1, struct Location *loc2) -{ - long off_x = loc2->lng - loc1->lng; - long off_y = (loc2->lat - loc1->lat) * scaleLongUp; - long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; - if (bearing < 0) bearing += 36000; - return bearing; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde deleted file mode 100644 index 89b81648e5..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde +++ /dev/null @@ -1,180 +0,0 @@ -//Function that will read the radio data, limit servos and trigger a failsafe -// ---------------------------------------------------------------------------- -byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling - -void read_radio() -{ - ch1_temp = APM_RC.InputCh(CH_ROLL); - ch2_temp = APM_RC.InputCh(CH_PITCH); - - if(mix_mode == 0){ - radio_in[CH_ROLL] = ch1_temp; - radio_in[CH_PITCH] = ch2_temp; - }else{ - radio_in[CH_ROLL] = reverse_elevons * (reverse_ch2_elevon * (ch2_temp - elevon2_trim) - reverse_ch1_elevon * (ch1_temp - elevon1_trim)) / 2 + 1500; - radio_in[CH_PITCH] = (reverse_ch2_elevon * (ch2_temp - elevon2_trim) + reverse_ch1_elevon * (ch1_temp - elevon1_trim)) / 2 + 1500; - } - - for (int y = 2; y < 8; y++) - radio_in[y] = APM_RC.InputCh(y); - - #if THROTTLE_REVERSE == 1 - radio_in[CH_THROTTLE] = radio_max[CH_THROTTLE] + radio_min[CH_THROTTLE] - radio_in[CH_THROTTLE]; - #endif - - throttle_failsafe(radio_in[CH_THROTTLE]); - servo_out[CH_THROTTLE] = ((float)(radio_in[CH_THROTTLE] - radio_min[CH_THROTTLE]) / (float)(radio_max[CH_THROTTLE] - radio_min[CH_THROTTLE])) * 100; - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); -} - -void throttle_failsafe(int pwm) -{ - if(throttle_failsafe_enabled == 0) - return; - - //check for failsafe and debounce funky reads - // ------------------------------------------ - if (pwm < throttle_failsafe_value){ - // we detect a failsafe from radio - // throttle has dropped below the mark - failsafeCounter++; - if (failsafeCounter == 9){ - SendDebug("MSG FS ON "); - SendDebugln(pwm, DEC); - }else if(failsafeCounter == 10) { - ch3_failsafe = true; - //set_failsafe(true); - //failsafeCounter = 10; - }else if (failsafeCounter > 10){ - failsafeCounter = 11; - } - - }else if(failsafeCounter > 0){ - // we are no longer in failsafe condition - // but we need to recover quickly - failsafeCounter--; - if (failsafeCounter > 3){ - failsafeCounter = 3; - } - if (failsafeCounter == 1){ - SendDebug("MSG FS OFF "); - SendDebugln(pwm, DEC); - }else if(failsafeCounter == 0) { - ch3_failsafe = false; - //set_failsafe(false); - //failsafeCounter = -1; - }else if (failsafeCounter <0){ - failsafeCounter = -1; - } - } -} - -void trim_control_surfaces() -{ - // Store control surface trim values - // --------------------------------- - if(mix_mode == 0){ - radio_trim[CH_ROLL] = radio_in[CH_ROLL]; - radio_trim[CH_PITCH] = radio_in[CH_PITCH]; - radio_trim[CH_RUDDER] = radio_in[CH_RUDDER]; - }else{ - elevon1_trim = ch1_temp; - elevon2_trim = ch2_temp; - //Recompute values here using new values for elevon1_trim and elevon2_trim - //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed - radio_trim[CH_ROLL] = 1500; - radio_trim[CH_PITCH] = 1500; - } - // disabled for now - //save_EEPROM_trims(); -} - -void trim_radio() -{ - for (int y = 0; y < 50; y++) { - read_radio(); - } - - // Store the trim values - // --------------------- - if(mix_mode == 0){ - for (int y = 0; y < 8; y++) radio_trim[y] = radio_in[y]; - }else{ - elevon1_trim = ch1_temp; - elevon2_trim = ch2_temp; - radio_trim[CH_ROLL] = 1500; - radio_trim[CH_PITCH] = 1500; - for (int y = 2; y < 8; y++) radio_trim[y] = radio_in[y]; - } - save_EEPROM_trims(); -} - - -#if SET_RADIO_LIMITS == 1 -void read_radio_limits() -{ - // set initial servo limits for calibration routine - // ------------------------------------------------- - radio_min[CH_ROLL] = radio_in[CH_ROLL] - 150; - radio_max[CH_ROLL] = radio_in[CH_ROLL] + 150; - - radio_min[CH_PITCH] = radio_in[CH_PITCH] - 150; - radio_max[CH_PITCH] = radio_in[CH_PITCH] + 150; - - // vars for the radio config routine - // --------------------------------- - int counter = 0; - long reminder; - reminder = millis() - 10000; - - // Allows user to set stick limits and calibrate the IR - // ---------------------------------------------------- - while(counter < 50){ - - if (millis() - reminder >= 10000) { // Remind user every 10 seconds what is going on - send_message(SEVERITY_LOW,"Reading radio limits:"); - send_message(SEVERITY_LOW,"Move sticks to: upper right and lower Left"); - send_message(SEVERITY_LOW,"To Continue, hold the stick in the corner for 2 seconds."); - send_message(SEVERITY_LOW," "); - //print_radio(); - demo_servos(1); - reminder = millis(); - } - - delay(40); - read_radio(); - - // AutoSet servo limits - // -------------------- - if (radio_in[CH_ROLL] > 1000 && radio_in[CH_ROLL] < 2000){ - radio_min[CH_ROLL] = min(radio_in[CH_ROLL], radio_min[CH_ROLL]); - radio_max[CH_ROLL] = max(radio_in[CH_ROLL], radio_max[CH_ROLL]); - } - - if (radio_in[CH_PITCH] > 1000 && radio_in[CH_PITCH]< 2000){ - radio_min[CH_PITCH] = min(radio_in[CH_PITCH], radio_min[CH_PITCH]); - radio_max[CH_PITCH] = max(radio_in[CH_PITCH], radio_max[CH_PITCH]); - } - if(radio_in[CH_PITCH] < (radio_min[CH_PITCH] + 30) || radio_in[CH_PITCH] > (radio_max[CH_PITCH] -30)){ - SendDebug("."); - counter++; - }else{ - if (counter > 0) - counter--; - } - } - - // contstrain min values - // --------------------- - radio_min[CH_ROLL] = constrain(radio_min[CH_ROLL], 800, 2200); - radio_max[CH_ROLL] = constrain(radio_max[CH_ROLL], 800, 2200); - radio_min[CH_PITCH] = constrain(radio_min[CH_PITCH], 800, 2200); - radio_max[CH_PITCH] = constrain(radio_max[CH_PITCH], 800, 2200); - - SendDebugln(" "); -} -#endif - - - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde deleted file mode 100644 index c0b12e618e..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde +++ /dev/null @@ -1,65 +0,0 @@ -void ReadSCP1000(void) {} - - -void read_airpressure(void){ - double x; - - APM_BMP085.Read(); // Get new data from absolute pressure sensor - abs_press = APM_BMP085.Press; - abs_press_filt = (abs_press); // + 2l * abs_press_filt) / 3l; // Light filtering - //temperature = (temperature * 9 + temp_unfilt) / 10; We will just use the ground temp for the altitude calculation - - double p = (double)abs_press_gnd / (double)abs_press_filt; - double temp = (float)ground_temperature / 10.f + 273.15f; - x = log(p) * temp * 29271.267f; - //x = log(p) * temp * 29.271267 * 1000; - press_alt = (long)(x / 10) + ground_alt; // Pressure altitude in centimeters - // Need to add comments for theory..... -} - -// in M/S * 100 -void read_airspeed(void) -{ - #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed - airpressure_raw = ((float)APM_ADC.Ch(AIRSPEED_CH) * .25) + (airpressure_raw * .75); - airpressure = (int)airpressure_raw - airpressure_offset; - airpressure = max(airpressure, 0); - airspeed = sqrt((float)airpressure * airspeed_ratio) * 100; - #endif - - calc_airspeed_errors(); -} - -#if BATTERY_EVENT == 1 -void read_battery(void) -{ - battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; - battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; - battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9; - battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9; - - #if BATTERY_TYPE == 0 - if(battery_voltage3 < LOW_VOLTAGE) - low_battery_event(); - battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream - #endif - - #if BATTERY_TYPE == 1 - if(battery_voltage4 < LOW_VOLTAGE) - low_battery_event(); - battery_voltage = battery_voltage4; // set total battery voltage, for telemetry stream - #endif -} -#endif - -void zero_airspeed(void) -{ - airpressure_raw = (float)APM_ADC.Ch(AIRSPEED_CH); - for(int c = 0; c < 50; c++){ - delay(20); - airpressure_raw = (airpressure_raw * .90) + ((float)APM_ADC.Ch(AIRSPEED_CH) * .10); - } - airpressure_offset = airpressure_raw; -} - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde deleted file mode 100644 index 4f29457ee1..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde +++ /dev/null @@ -1,410 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Functions called from the setup menu -static int8_t setup_radio(uint8_t argc, const Menu::arg *argv); -static int8_t setup_show(uint8_t argc, const Menu::arg *argv); -static int8_t setup_factory(uint8_t argc, const Menu::arg *argv); -static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv); - -// Command/function table for the setup menu -const struct Menu::command setup_menu_commands[] PROGMEM = { - // command function called - // ======= =============== - {"reset", setup_factory}, - {"radio", setup_radio}, - {"modes", setup_flightmodes}, - {"show", setup_show}, -}; - -// Create the setup menu object. -MENU(setup_menu, "setup", setup_menu_commands); - -// Called from the top-level menu to run the setup menu. -int8_t -setup_mode(uint8_t argc, const Menu::arg *argv) -{ - // Give the user some guidance - Serial.printf_P(PSTR("Setup Mode\n" - "\n" - "IMPORTANT: if you have not previously set this system up, use the\n" - "'reset' command to initialize the EEPROM to sensible default values\n" - "and then the 'radio' command to configure for your radio.\n" - "\n")); - - // Run the setup menu. When the menu exits, we will return to the main menu. - setup_menu.run(); -} - -// Print the current configuration. -// Called by the setup menu 'show' command. -static int8_t -setup_show(uint8_t argc, const Menu::arg *argv) -{ - uint8_t i; - - Serial.printf_P(PSTR("\nRadio:\n")); - read_EEPROM_radio_minmax(); - for(i = 0; i < 8; i++) - Serial.printf_P(PSTR("CH%d: %d | %d\n"), i + 1, radio_min[i], radio_max[i]); - - Serial.printf_P(PSTR("\nGains:\n")); - read_EEPROM_gains(); - - for(i = 0; i < 8; i++){ - Serial.printf_P(PSTR("P,I,D,iMax:")); - Serial.print(kp[i],3); - Serial.print(","); - Serial.print(ki[i],3); - Serial.print(","); - Serial.print(kd[i],3); - Serial.print(","); - Serial.println(integrator_max[i]/100,DEC); - } - - Serial.printf_P(PSTR("kff:")); - Serial.print(kff[0],3); - Serial.print(","); - Serial.print(kff[1],3); - Serial.print(","); - Serial.println(kff[2],3); - - Serial.printf_P(PSTR("XTRACK_GAIN:")); - Serial.println(x_track_gain,DEC); - - Serial.printf_P(PSTR("XTRACK_ENTRY_ANGLE: %d\n"), x_track_angle); - Serial.printf_P(PSTR("HEAD_MAX: %d\n"), head_max); - Serial.printf_P(PSTR("PITCH_MAX: %d\n"), pitch_max); - Serial.printf_P(PSTR("PITCH_MIN: %d\n"), pitch_min); - - read_user_configs(); - Serial.printf_P(PSTR("\nUser config:\n")); - Serial.printf_P(PSTR("airspeed_cruise: %d\n"), airspeed_cruise); - Serial.printf_P(PSTR("airspeed_fbw_min: %d\n"), airspeed_fbw_min); - Serial.printf_P(PSTR("airspeed_fbw_max: %d\n"), airspeed_fbw_max); - Serial.printf_P(PSTR("airspeed_ratio: ")); - Serial.println(airspeed_ratio, 4); - - Serial.printf_P(PSTR("throttle_min: %d\n"), throttle_min); - Serial.printf_P(PSTR("throttle_max: %d\n"), throttle_max); - Serial.printf_P(PSTR("throttle_cruise: %d\n"), throttle_cruise); - Serial.printf_P(PSTR("throttle_failsafe_enabled: %d\n"), throttle_failsafe_enabled); - Serial.printf_P(PSTR("throttle_failsafe_value: %d\n"), throttle_failsafe_value); - Serial.printf_P(PSTR("flight_mode_channel: %d\n"), flight_mode_channel+1); //Add 1 to flight_mode_channel to change 0-based channels to 1-based channels - Serial.printf_P(PSTR("auto_trim: %d\n"), auto_trim); - Serial.printf_P(PSTR("log_bitmask: %d\n\n"), log_bitmask); - //Serial.printf_P(PSTR("Switch settings:\n")); - //for(i = 0; i < 6; i++){ - // print_switch(i+1, flight_modes[i]); - //} - - return(0); -} - -// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). -// Called by the setup menu 'factoryreset' command. -static int8_t -setup_factory(uint8_t argc, const Menu::arg *argv) -{ - uint8_t i; - int c; - - Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); - - do { - c = Serial.read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) - return(-1); - - Serial.printf_P(PSTR("\nFACTORY RESET\n\n")); - - wp_radius = WP_RADIUS_DEFAULT; - loiter_radius = LOITER_RADIUS_DEFAULT; - - save_EEPROM_waypoint_info(); - - radio_min[CH_1] = 0; - radio_min[CH_2] = 0; - radio_min[CH_3] = 0; - radio_min[CH_4] = 0; - radio_min[CH_5] = CH5_MIN; - radio_min[CH_6] = CH6_MIN; - radio_min[CH_7] = CH7_MIN; - radio_min[CH_8] = CH8_MIN; - - radio_max[CH_1] = 0; - radio_max[CH_2] = 0; - radio_max[CH_3] = 0; - radio_max[CH_4] = 0; - radio_max[CH_5] = CH5_MAX; - radio_max[CH_6] = CH6_MAX; - radio_max[CH_7] = CH7_MAX; - radio_max[CH_8] = CH8_MAX; - - save_EEPROM_radio_minmax(); - - kp[0] = SERVO_ROLL_P; - kp[1] = SERVO_PITCH_P; - kp[2] = SERVO_YAW_P; - kp[3] = NAV_ROLL_P; - kp[4] = NAV_PITCH_ASP_P; - kp[5] = NAV_PITCH_ALT_P; - kp[6] = THROTTLE_TE_P; - kp[7] = THROTTLE_ALT_P; - - ki[0] = SERVO_ROLL_I; - ki[1] = SERVO_PITCH_I; - ki[2] = SERVO_YAW_I; - ki[3] = NAV_ROLL_I; - ki[4] = NAV_PITCH_ASP_I; - ki[5] = NAV_PITCH_ALT_I; - ki[6] = THROTTLE_TE_I; - ki[7] = THROTTLE_ALT_I; - - kd[0] = SERVO_ROLL_D; - kd[1] = SERVO_PITCH_D; - kd[2] = SERVO_YAW_D; - kd[3] = NAV_ROLL_D; - kd[4] = NAV_PITCH_ASP_D; - kd[5] = NAV_PITCH_ALT_D; - kd[6] = THROTTLE_TE_D; - kd[7] = THROTTLE_ALT_D; - - integrator_max[0] = SERVO_ROLL_INT_MAX; - integrator_max[1] = SERVO_PITCH_INT_MAX; - integrator_max[2] = SERVO_YAW_INT_MAX; - integrator_max[3] = NAV_ROLL_INT_MAX; - integrator_max[4] = NAV_PITCH_ASP_INT_MAX; - integrator_max[5] = NAV_PITCH_ALT_INT_MAX; - integrator_max[6] = THROTTLE_TE_INT_MAX; - integrator_max[7] = THROTTLE_ALT_INT_MAX; - - kff[0] = PITCH_COMP; - kff[1] = RUDDER_MIX; - kff[2] = P_TO_T; - - for(i = 0; i < 8; i++){ - // scale input to deg * 100; - integrator_max[i] *= 100; - } - x_track_gain = XTRACK_GAIN * 100; - x_track_angle = XTRACK_ENTRY_ANGLE * 100; - altitude_mix = ALTITUDE_MIX; - - head_max = HEAD_MAX * 100; - pitch_max = PITCH_MAX * 100; - pitch_min = PITCH_MIN * 100; - - save_EEPROM_gains(); - - airspeed_cruise = AIRSPEED_CRUISE*100; - airspeed_fbw_min = AIRSPEED_FBW_MIN; - airspeed_fbw_max = AIRSPEED_FBW_MAX; - airspeed_ratio = AIRSPEED_RATIO; - throttle_min = THROTTLE_MIN; - throttle_max = THROTTLE_MAX; - throttle_cruise = THROTTLE_CRUISE; - throttle_failsafe_enabled = THROTTLE_FAILSAFE; - throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION; - throttle_failsafe_value = THROTTLE_FS_VALUE; - //flight_mode_channel = FLIGHT_MODE_CHANNEL - 1; - auto_trim = AUTO_TRIM; - - flight_modes[0] = FLIGHT_MODE_1; - flight_modes[1] = FLIGHT_MODE_2; - flight_modes[2] = FLIGHT_MODE_3; - flight_modes[3] = FLIGHT_MODE_4; - flight_modes[4] = FLIGHT_MODE_5; - flight_modes[5] = FLIGHT_MODE_6; - save_EEPROM_flight_modes(); - - // convenience macro for testing LOG_* and setting LOGBIT_* -#define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0) - log_bitmask = - LOGBIT(ATTITUDE_FAST) | - LOGBIT(ATTITUDE_MED) | - LOGBIT(GPS) | - LOGBIT(PM) | - LOGBIT(CTUN) | - LOGBIT(NTUN) | - LOGBIT(MODE) | - LOGBIT(RAW) | - LOGBIT(CMD); -#undef LOGBIT - - save_user_configs(); - - return(0); -} - -// Perform radio setup. -// Called by the setup menu 'radio' command. -static int8_t -setup_radio(uint8_t argc, const Menu::arg *argv) -{ - Serial.println("\n\nRadio Setup:"); - uint8_t i; - - for(i = 0; i < 100;i++){ - delay(20); - read_radio(); - } - - if(radio_in[CH_ROLL] < 500){ - while(1){ - Serial.print("Radio error"); - delay(1000); - // stop here - } - } - - for(i = 0; i < 4; i++){ - radio_min[i] = radio_in[i]; - radio_max[i] = radio_in[i]; - } - - Serial.printf_P(PSTR("\nMove both sticks to each corner. Hit Enter to save: ")); - while(1){ - - delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - - for(i = 0; i < 4; i++){ - radio_min[i] = min(radio_min[i], radio_in[i]); - radio_max[i] = max(radio_max[i], radio_in[i]); - } - - if(Serial.available() > 0){ - Serial.flush(); - Serial.println("Saving:"); - - save_EEPROM_radio_minmax(); - delay(100); - read_EEPROM_radio_minmax(); - - for(i = 0; i < 8; i++) - Serial.printf_P(PSTR("CH%d: %d | %d\n"), i + 1, radio_min[i], radio_max[i]); - break; - } - } - - return(0); -} - - -static int8_t -setup_flightmodes(uint8_t argc, const Menu::arg *argv) -{ - byte switchPosition, oldSwitchPosition, mode; - - Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); - print_hit_enter(); - trim_radio(); - - while(1){ - delay(20); - read_radio(); - switchPosition = readSwitch(); - - - // look for control switch change - if (oldSwitchPosition != switchPosition){ - - // Override position 5 - if(switchPosition > 4){ - mode = flight_modes[switchPosition] = MANUAL; - }else{ - // update our current mode - mode = flight_modes[switchPosition]; - } - - // update the user - print_switch(switchPosition, mode); - - // Remember switch position - oldSwitchPosition = switchPosition; - } - - // look for stick input - if (radio_input_switch() == true){ - switch(mode){ - case MANUAL: - mode = STABILIZE; - break; - - case STABILIZE: - mode = FLY_BY_WIRE_A; - break; - - case FLY_BY_WIRE_A: - mode = FLY_BY_WIRE_B; - break; - - case FLY_BY_WIRE_B: - mode = AUTO; - break; - - case AUTO: - mode = RTL; - break; - - case RTL: - mode = LOITER; - break; - - case LOITER: - mode = MANUAL; - break; - - default: - mode = MANUAL; - break; - } - - // 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_EEPROM_flight_modes(); - return (0); - } - } -} - -void -print_switch(byte p, byte m) -{ - Serial.printf_P(PSTR("Pos %d: "),p); - Serial.println(flight_mode_strings[m]); -} - -boolean -radio_input_switch(void) -{ - static byte bouncer; - - if (abs(radio_in[0] - radio_trim[0]) > 200) - bouncer = 10; - - if (bouncer > 0) - bouncer--; - - if (bouncer == 1){ - return true; - }else{ - return false; - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/system.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/system.pde deleted file mode 100644 index 56a44067da..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/system.pde +++ /dev/null @@ -1,491 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -/***************************************************************************** -The init_ardupilot function processes everything we need for an in - air restart - We will determine later if we are actually on the ground and process a - ground start in that case. - -*****************************************************************************/ - -// Functions called from the top-level menu -extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Commands:\n" - " logs log readback/setup mode\n" - " setup setup mode\n" - " test test mode\n" - "\n" - "Move the slide switch and reset to FLY.\n" - "\n")); - return(0); -} - -// Command/function table for the top-level menu. -const struct Menu::command main_menu_commands[] PROGMEM = { -// command function called -// ======= =============== - {"logs", process_logs}, - {"setup", setup_mode}, - {"test", test_mode}, - {"help", main_menu_help} -}; - -// Create the top-level menu object. -MENU(main_menu, "ArduPilotMega", main_menu_commands); - -void init_ardupilot() -{ - - byte last_log_num; - int last_log_start; - int last_log_end; - - // Console serial port - // - // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. - // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - Serial.begin(SERIAL0_BAUD, 128, 128); - - // GPS serial port. - // - // Not used if the IMU/X-Plane GPS is in use. - // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // -#if GPS_PROTOCOL != GPS_PROTOCOL_IMU - Serial1.begin(38400, 128, 16); -#endif - - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // -#if GCS_PORT == 3 - Serial3.begin(SERIAL3_BAUD, 128, 128); -#endif - - Serial.printf_P(PSTR("\n\n" - "Init ArduPilotMega X-DIY\n\n" -#if TELEMETRY_PORT == 3 - "Telemetry is on the xbee port\n" -#endif - "freeRAM: %d\n"), freeRAM()); - - APM_RC.Init(); // APM Radio initialization - read_EEPROM_startup(); // Read critical config information to start - - APM_ADC.Init(); // APM ADC library initialization - APM_BMP085.Init(); // APM Abs Pressure sensor initialization - DataFlash.Init(); // DataFlash log initialization - GPS.init(); // GPS Initialization - - #if MAGNETOMETER == ENABLED - APM_Compass.Init(); // I2C initialization - #endif - - APM_RC.OutputCh(CH_ROLL, radio_trim[CH_ROLL]); // Initialization of servo outputs - APM_RC.OutputCh(CH_PITCH, radio_trim[CH_PITCH]); - APM_RC.OutputCh(CH_THROTTLE, radio_trim[CH_THROTTLE]); - APM_RC.OutputCh(CH_RUDDER, radio_trim[CH_RUDDER]); - - pinMode(C_LED_PIN, OUTPUT); // GPS status LED - pinMode(A_LED_PIN, OUTPUT); // GPS status LED - pinMode(B_LED_PIN, OUTPUT); // GPS status LED - pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode - pinMode(PUSHBUTTON_PIN, INPUT); // unused - - - // If the switch is in 'menu' mode, run the main menu. - // - // Since we can't be sure that the setup or test mode won't leave - // the system in an odd state, we don't let the user exit the top - // menu; they must reset in order to fly. - // - if (digitalRead(SLIDE_SWITCH_PIN) == 0) { - digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED - Serial.printf_P(PSTR("\n" - "Entering interactive setup mode...\n" - "\n" - "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n" - "Type 'help' to list commands, 'exit' to leave a submenu.\n" - "Visit the 'setup' menu for first-time configuration.\n")); - for (;;) { - Serial.printf_P(PSTR("\n" - "Move the slide switch and reset to FLY.\n" - "\n")); - main_menu.run(); - } - } - - - if(log_bitmask > 0){ - // Here we will check on the length of the last log - // We don't want to create a bunch of little logs due to powering on and off - last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); - last_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(last_log_num - 1) * 0x02)); - last_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); - - if(last_log_num == 0) { - // The log space is empty. Start a write session on page 1 - DataFlash.StartWrite(1); - eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (1)); - eeprom_write_word((uint16_t *) EE_LOG_1_START, (1)); - - } else if (last_log_end <= last_log_start + 10) { - // The last log is small. We consider it junk. Overwrite it. - DataFlash.StartWrite(last_log_start); - - } else { - // The last log is valid. Start a new log - if(last_log_num >= 19) { - Serial.println("Number of log files exceeds max. Log 19 will be overwritten."); - last_log_num --; - } - DataFlash.StartWrite(last_log_end + 1); - eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (last_log_num + 1)); - eeprom_write_word((uint16_t *) (EE_LOG_1_START+(last_log_num)*0x02), (last_log_end + 1)); - } - } - - // read in the flight switches - update_servo_switches(); - - if(DEBUG_SUBSYSTEM > 0){ - debug_subsystem(); - - } else if (ENABLE_AIR_START == 1) { - // Perform an air start and get back to flying - send_message(SEVERITY_LOW," AIR START"); - - // Get necessary data from EEPROM - //---------------- - read_EEPROM_airstart_critical(); - - // This delay is important for the APM_RC library to work. We need some time for the comm between the 328 and 1280 to be established. - int old_pulse = 0; - while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(flight_mode_channel)) > 5 || APM_RC.InputCh(flight_mode_channel) == 1000 || APM_RC.InputCh(flight_mode_channel) == 1200)) { - old_pulse = APM_RC.InputCh(flight_mode_channel); - delay(25); - } - if (log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_AIRSTART_MSG); - reload_commands(); // Get set to resume AUTO from where we left off - - }else { - startup_ground(); - if (log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - } - - // set the correct flight mode - // --------------------------- - reset_control_switch(); -} - -//******************************************************************************** -//This function does all the calibrations, etc. that we need during a ground start -//******************************************************************************** -void startup_ground(void) -{ - send_message(SEVERITY_LOW," GROUND START"); - - #if(GROUND_START_DELAY > 0) - send_message(SEVERITY_LOW," With Delay"); - delay(GROUND_START_DELAY * 1000); - #endif - - // Output waypoints for confirmation - // -------------------------------- - for(int i = 1; i < wp_total + 1; i++) { - send_message(MSG_COMMAND, i); - } - - // Makes the servos wiggle - // step 1 = 1 wiggle - // ----------------------- - demo_servos(1); - - //IMU ground start - //------------------------ -#if GPS_PROTOCOL != GPS_PROTOCOL_IMU - startup_IMU_ground(); -#endif - - - // read the radio to set trims - // --------------------------- - trim_radio(); - - #if AIRSPEED_SENSOR == 1 - // initialize airspeed sensor - // -------------------------- - zero_airspeed(); - send_message(SEVERITY_LOW," zero airspeed calibrated"); - #else - send_message(SEVERITY_LOW," NO airspeed"); - #endif - - // Save the settings for in-air restart - // ------------------------------------ - save_EEPROM_groundstart(); - - // initialize commands - // ------------------- - init_commands(); - -} - -void set_mode(byte mode) -{ - if(control_mode == mode){ - // don't switch modes if we are already in the correct mode. - return; - } - if(auto_trim > 0 || control_mode == MANUAL) - trim_control_surfaces(); - - control_mode = mode; - crash_timer = 0; - - switch(control_mode) - { - case MANUAL: - break; - - case STABILIZE: - break; - - case FLY_BY_WIRE_A: - break; - - case FLY_BY_WIRE_B: - break; - - case AUTO: - update_auto(); - break; - - case RTL: - return_to_launch(); - break; - - case LOITER: - loiter_at_location(); - break; - - case TAKEOFF: - break; - - case LAND: - break; - - default: - return_to_launch(); - break; - } - - // output control mode to the ground station - send_message(MSG_HEARTBEAT); - - if (log_bitmask & MASK_LOG_MODE) - Log_Write_Mode(control_mode); -} - -void set_failsafe(boolean mode) -{ - // only act on changes - // ------------------- - if(failsafe != mode){ - - // store the value so we don't trip the gate twice - // ----------------------------------------------- - failsafe = mode; - - if (failsafe == false){ - // We're back in radio contact - // --------------------------- - - // re-read the switch so we can return to our preferred mode - reset_control_switch(); - - // Reset control integrators - // --------------------- - reset_I(); - - }else{ - // We've lost radio contact - // ------------------------ - // nothing to do right now - } - - // Let the user know what's up so they can override the behavior - // ------------------------------------------------------------- - failsafe_event(); - } -} - - -void startup_IMU_ground(void) -{ - uint16_t store = 0; - int flashcount = 0; - // 12345678901234567890123456789012 - jeti_status(" **** INIT **** Warming up ADC"); - jeti_update(); - SendDebugln(" Warming up ADC..."); - - for(int c = 0; c < ADC_WARM_CYCLES; c++) - { - digitalWrite(C_LED_PIN, LOW); - digitalWrite(A_LED_PIN, HIGH); - digitalWrite(B_LED_PIN, LOW); - delay(50); - Read_adc_raw(); - digitalWrite(C_LED_PIN, HIGH); - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, HIGH); - delay(50); - } - - // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! - // ----------------------- - demo_servos(2); - SendDebugln(" Beginning IMU calibration; do not move plane"); - // 12345678901234567890123456789012 - jeti_status(" **** INIT **** Do not move!!!"); - jeti_update(); - - - for(int y = 0; y <= 5; y++){ // Read first initial ADC values for offset. - AN_OFFSET[y] = AN[y]; - } - - APM_BMP085.Read(); // Get initial data from absolute pressure sensor - abs_press_gnd = APM_BMP085.Press; - ground_temperature = APM_BMP085.Temp; - delay(20); - // *********** - - for(int i = 0; i < 400; i++){ // We take some readings... - Read_adc_raw(); - for(int y = 0; y <= 5; y++) // Read initial ADC values for offset (averaging). - AN_OFFSET[y] = AN_OFFSET[y] * 0.9 + AN[y] * 0.1; - - APM_BMP085.Read(); // Get initial data from absolute pressure sensor - abs_press_gnd = (abs_press_gnd * 9l + APM_BMP085.Press) / 10l; - ground_temperature = (ground_temperature * 9 + APM_BMP085.Temp) / 10; - - delay(20); - if(flashcount == 5) { - digitalWrite(C_LED_PIN, LOW); - digitalWrite(A_LED_PIN, HIGH); - digitalWrite(B_LED_PIN, LOW); - } - if(flashcount >= 10) { - flashcount = 0; - digitalWrite(C_LED_PIN, HIGH); - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, HIGH); - } - flashcount++; - - } - SendDebugln(" Calibration complete."); - digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready - digitalWrite(A_LED_PIN, LOW); - digitalWrite(C_LED_PIN, LOW); - - AN_OFFSET[5] -= GRAVITY * SENSOR_SIGN[5]; -/* - Serial.print ("Offsets "); - Serial.print (AN_OFFSET[0]); - Serial.print (" "); - Serial.print (AN_OFFSET[1]); - Serial.print (" "); - Serial.print (AN_OFFSET[2]); - Serial.print (" "); - Serial.print (AN_OFFSET[3]); - Serial.print (" "); - Serial.print (AN_OFFSET[4]); - Serial.print (" "); - Serial.println (AN_OFFSET[5]); -*/ -} - - -void update_GPS_light(void) -{ - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - switch (GPS.status()) { - case(2): - digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case(1): - if (GPS.valid_read == true){ - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light){ - digitalWrite(C_LED_PIN, LOW); - } else { - digitalWrite(C_LED_PIN, HIGH); - } - GPS.valid_read = false; - } - break; - - default: - digitalWrite(C_LED_PIN, LOW); - break; - } -} - - - -void resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; - gyro_sat_count = 0; - adc_constraints = 0; - renorm_sqrt_count = 0; - renorm_blowup_count = 0; - gps_fix_count = 0; - perf_mon_timer = millis(); -} - - -/* This function gets the current value of the heap and stack pointers. -* The stack pointer starts at the top of RAM and grows downwards. The heap pointer -* starts just above the static variables etc. and grows upwards. SP should always -* be larger than HP or you'll be in big trouble! The smaller the gap, the more -* careful you need to be. Julian Gall 6 - Feb - 2009. -*/ -unsigned long freeRAM() { - uint8_t * heapptr, * stackptr; - stackptr = (uint8_t *)malloc(4); // use stackptr temporarily - heapptr = stackptr; // save value of heap pointer - free(stackptr); // free up the memory again (sets stackptr to 0) - stackptr = (uint8_t *)(SP); // save value of stack pointer - return stackptr - heapptr; -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/test.pde deleted file mode 100644 index d6791b4291..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/X-DIY/test.pde +++ /dev/null @@ -1,433 +0,0 @@ -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t test_radio(uint8_t argc, const Menu::arg *argv); -static int8_t test_gps(uint8_t argc, const Menu::arg *argv); -static int8_t test_imu(uint8_t argc, const Menu::arg *argv); -static int8_t test_gyro(uint8_t argc, const Menu::arg *argv); -static int8_t test_battery(uint8_t argc, const Menu::arg *argv); -static int8_t test_relay(uint8_t argc, const Menu::arg *argv); -static int8_t test_wp(uint8_t argc, const Menu::arg *argv); -static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); -static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); -static int8_t test_mag(uint8_t argc, const Menu::arg *argv); -static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); -static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_jeti(uint8_t argc, const Menu::arg *argv); - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -/*static int8_t help_test(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("\n" - "Commands:\n" - " radio\n" - " servos\n" - " gps\n" - " imu\n" - " battery\n" - "\n")); -}*/ - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -const struct Menu::command test_menu_commands[] PROGMEM = { - {"radio", test_radio}, - {"gps", test_gps}, - {"imu", test_imu}, - {"gyro", test_gyro}, - {"battery", test_battery}, - {"relay", test_relay}, - {"waypoints", test_wp}, - {"airspeed", test_airspeed}, - {"airpressure", test_pressure}, - {"compass", test_mag}, - {"xbee", test_xbee}, - {"eedump", test_eedump}, - {"jeti", test_jeti}, -}; - -// A Macro to create the Menu -MENU(test_menu, "test", test_menu_commands); - -int8_t -test_mode(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Test Mode\n\n")); - test_menu.run(); -} - -static int8_t -test_eedump(uint8_t argc, const Menu::arg *argv) -{ - int i, j; - - // hexdump the EEPROM - for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { - Serial.printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); - Serial.println(); - } - return(0); -} - -static int8_t -test_radio(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - #if THROTTLE_REVERSE == 1 - Serial.println("Throttle is reversed in config: "); - delay(1000); - #endif - - // read the radio to set trims - // --------------------------- - trim_radio(); - - while(1){ - delay(20); - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- - read_radio(); - update_servo_switches(); - - servo_out[CH_ROLL] = reverse_roll * (radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 9; - servo_out[CH_PITCH] = reverse_pitch * (radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 9; - servo_out[CH_RUDDER] = reverse_rudder * (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 9; - - // write out the servo PWM values - // ------------------------------ - set_servos_4(); - - - for(int y = 4; y < 8; y++) { - radio_out[y] = constrain(radio_in[y], radio_min[y], radio_max[y]); - APM_RC.OutputCh(y, radio_out[y]); // send to Servos - } - Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\t"), radio_in[CH_1], radio_in[CH_2], radio_in[CH_3], radio_in[CH_4], radio_in[CH_5], radio_in[CH_6], radio_in[CH_7], radio_in[CH_8]); - Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (servo_out[CH_ROLL]/100), (servo_out[CH_PITCH]/100), servo_out[CH_THROTTLE], (servo_out[CH_RUDDER]/100)); - - if(Serial.available() > 0){ - return (0); - } - } -} - - -static int8_t -test_gps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(333); - - // Blink GPS LED if we don't have a fix - // ------------------------------------ - update_GPS_light(); - - GPS.update(); - - if (GPS.new_data){ - Serial.print("Lat:"); - Serial.print((float)GPS.latitude/10000000, 10); - Serial.print(" Lon:"); - Serial.print((float)GPS.longitude/10000000, 10); - Serial.printf_P(PSTR("alt %dm, #sats: %d\n"), GPS.altitude/100, GPS.num_sats); - }else{ - Serial.print("."); - } - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_imu(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Calibrating.")); - startup_IMU_ground(); - print_hit_enter(); - delay(1000); - - while(1){ - delay(20); - read_AHRS(); - - // We are using the IMU - // --------------------- - Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_battery(uint8_t argc, const Menu::arg *argv) -{ -#if BATTERY_EVENT == 1 - for (int i = 0; i < 20; i++){ - delay(20); - read_battery(); - } - Serial.printf_P(PSTR("Volts: 1:")); - Serial.print(battery_voltage1, 4); - Serial.print(" 2:"); - Serial.print(battery_voltage2, 4); - Serial.print(" 3:"); - Serial.print(battery_voltage3, 4); - Serial.print(" 4:"); - Serial.println(battery_voltage4, 4); -#else - Serial.printf_P(PSTR("Not enabled\n")); - -#endif - return (0); -} - - -static int8_t -test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - //DDRL |= B00000100; - - while(1){ - Serial.println(int DDRL); - Serial.println(int PORTL); - Serial.println("Relay A"); - relay_A(); - delay(500); - if(Serial.available() > 0){ - return (0); - } - - Serial.println("Relay B"); - relay_B(); - delay(500); - if(Serial.available() > 0){ - return (0); - } - } -} - - -static int8_t -test_gyro(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - delay(200); - - while(1){ - for (int i = 0; i < 6; i++) { - AN[i] = APM_ADC.Ch(sensors[i]); - } - Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), (int)AN[0], (int)AN[1], (int)AN[2], (int)AN[3], (int)AN[4], (int)AN[5]); - delay(100); - - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_wp(uint8_t argc, const Menu::arg *argv) -{ - delay(1000); - ground_alt = 0; - read_EEPROM_waypoint_info(); - - uint8_t options = eeprom_read_byte((const uint8_t *) EE_CONFIG); - int32_t hold = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME); - - // save the alitude above home option - if(options & HOLD_ALT_ABOVE_HOME){ - Serial.printf_P(PSTR("Hold altitude of %dm\n"), hold/100); - }else{ - Serial.printf_P(PSTR("Hold current altitude\n")); - } - - Serial.printf_P(PSTR("%d waypoints\n"), wp_total); - Serial.printf_P(PSTR("Hit radius: %d\n"), wp_radius); - Serial.printf_P(PSTR("Loiter radius: %d\n\n"), loiter_radius); - - for(byte i = 0; i <= wp_total; i++){ - struct Location temp = get_wp_with_index(i); - print_waypoint(&temp, i); - } - - return (0); -} - - -static int8_t -test_airspeed(uint8_t argc, const Menu::arg *argv) -{ -#if AIRSPEED_SENSOR == DISABLED - Serial.printf_P(PSTR("airspeed disabled\n")); - return (0); -#else - print_hit_enter(); - zero_airspeed(); - - while(1){ - delay(20); - read_airspeed(); - Serial.printf_P(PSTR("%dm/s\n"),airspeed/100); - - if(Serial.available() > 0){ - return (0); - } - } -#endif -} - -static int8_t -test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); - while(1){ - delay(250); - // Timeout set high enough for X-CTU RSSI Calc over XBee @ 115200 - //Serial3.printf_P(PSTR("0123456789:;<=>?@ABCDEFGHIJKLMNO\n")); - //Serial.print("X"); - // Default 32bit data from X-CTU Range Test - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_pressure(uint8_t argc, const Menu::arg *argv) -{ - uint32_t sum = 0; - Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n")); - Serial.printf_P(PSTR("Note - the altitude displayed is relative to the start of this test\n")); - print_hit_enter(); - Serial.printf_P(PSTR("\nCalibrating....\n")); - for (int i=1;i<301;i++) { - read_airpressure(); - if(i>200)sum += abs_press_filt; - delay(10); - } - abs_press_gnd = (double)sum/100.0; - - ground_alt = 0; - while(1){ - delay(100); - read_airpressure(); - //Serial.printf_P(PSTR("Alt: %dm, Raw: %d\n"), press_alt / 100, abs_press_filt); // Someone needs to fix the formatting here for long integers -Serial.print("Altitude: "); -Serial.print(press_alt/100.0,2); -Serial.print(" Raw pressure value: "); -Serial.println(abs_press_filt); - if(Serial.available() > 0){ - return (0); - } - } -} - -static int8_t -test_mag(uint8_t argc, const Menu::arg *argv) -{ -#if MAGNETOMETER == DISABLED - Serial.printf_P(PSTR("Compass disabled\n")); - return (0); -#else - print_hit_enter(); - - while(1){ - delay(250); - APM_Compass.Read(); - APM_Compass.Calculate(0,0); - Serial.printf_P(PSTR("Heading: (")); - Serial.print(ToDeg(APM_Compass.Heading)); - Serial.printf_P(PSTR(") XYZ: (")); - Serial.print(APM_Compass.Mag_X); - Serial.print(comma); - Serial.print(APM_Compass.Mag_Y); - Serial.print(comma); - Serial.print(APM_Compass.Mag_Z); - Serial.println(")"); - if(Serial.available() > 0){ - return (0); - } - } -#endif -} - -static int8_t -test_jeti(uint8_t argc, const Menu::arg *argv) -{ - uint8_t switchPosition, m; - print_hit_enter(); - - JB.begin(); - //JB.print(" X-DIY"); - //JB.print(" Test mode"); - //JB.checkvalue(20000); - Serial.print("Jeti Box test: press any buttons\n\n"); - while(1){ - delay(200); - //Serial.println(JB.checkvalue(0)); - read_radio(); - switchPosition = readSwitch(); - m = flight_modes[switchPosition]; - JB.clear(1); - JB.print("Mode: "); - JB.print(flight_mode_strings[m]); - JB.clear(2); - switch (JB.readbuttons()) - { - case JB_key_right: - JB.print("rechts"); - Serial.print("rechts\n"); - break; - - case JB_key_left: - JB.print("links"); - Serial.print("links\n"); - break; - - case JB_key_up: - JB.print("hoch"); - Serial.print("hoch\n"); - break; - - case JB_key_down: - JB.print("runter"); - Serial.print("runter\n"); - break; - - case JB_key_left | JB_key_right: - JB.print("links-rechts"); - Serial.print("links-rechts\n"); - break; - } - - if(Serial.available() > 0){ - return (0); - } - } -} - -void print_hit_enter() -{ - Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp deleted file mode 100644 index 2543002d76..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/* - APM_ADC.cpp - ADC ADS7844 Library for Ardupilot Mega - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - Modified by John Ihlein 6/19/2010 to: - 1)Prevent overflow of adc_counter when more than 8 samples collected between reads. Probably - only an issue on initial read of ADC at program start. - 2)Reorder analog read order as follows: - p, q, r, ax, ay, az - - This library 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. - - External ADC ADS7844 is connected via Serial port 2 (in SPI mode) - TXD2 = MOSI = pin PH1 - RXD2 = MISO = pin PH0 - XCK2 = SCK = pin PH2 - Chip Select pin is PC4 (33) [PH6 (9)] - We are using the 16 clocks per conversion timming to increase efficiency (fast) - The sampling frequency is 400Hz (Timer2 overflow interrupt) - So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so - we have an 4x oversampling and averaging. - - Methods: - Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt) - Ch(ch_num) : Return the ADC channel value - - // HJI - Input definitions. USB connector assumed to be on the left, Rx and servo - // connector pins to the rear. IMU shield components facing up. These are board - // referenced sensor inputs, not device referenced. - On Ardupilot Mega Hardware, oriented as described above: - Channel 0 : yaw rate, r - Channel 1 : roll rate, p - Channel 2 : pitch rate, q - Channel 3 : x/y gyro temperature - Channel 4 : x acceleration, aX - Channel 5 : y acceleration, aY - Channel 6 : z acceleration, aZ - Channel 7 : Differential pressure sensor port - -*/ -extern "C" { - // AVR LibC Includes - #include - #include - #include "WConstants.h" -} - -#include "APM_ADC.h" - - -// Commands for reading ADC channels on ADS7844 -const unsigned char adc_cmd[9]= { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; -volatile long adc_value[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; -volatile unsigned char adc_counter[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; - -unsigned char ADC_SPI_transfer(unsigned char data) -{ - /* Wait for empty transmit buffer */ - while ( !( UCSR2A & (1<= 17) // HJI - Added this to prevent - { // overflow of adc_value - adc_value[ch] = 0; - adc_counter[ch] = 0; - } - adc_tmp = ADC_SPI_transfer(0)<<8; // Read first byte - adc_tmp |= ADC_SPI_transfer(adc_cmd[ch+1]); // Read second byte and send next command - adc_value[ch] += adc_tmp>>3; // Shift to 12 bits - adc_counter[ch]++; // Number of samples - } - bit_set(PORTC,4); // Disable Chip Select (PIN PC4) - //bit_clear(PORTL,6); // To test performance - TCNT2 = 104; // 400 Hz -} - - -// Constructors //////////////////////////////////////////////////////////////// -APM_ADC_Class::APM_ADC_Class() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void APM_ADC_Class::Init(void) -{ - unsigned char tmp; - - pinMode(ADC_CHIP_SELECT,OUTPUT); - - digitalWrite(ADC_CHIP_SELECT,HIGH); // Disable device (Chip select is active low) - - // Setup Serial Port2 in SPI mode - UBRR2 = 0; - DDRH |= (1<0) - result = adc_value[ch_num]/adc_counter[ch_num]; - else - result = 0; - adc_value[ch_num] = 0; // Initialize for next reading - adc_counter[ch_num] = 0; - sei(); - return(result); -} - -// make one instance for the user to use -APM_ADC_Class APM_ADC; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h deleted file mode 100644 index cea6102eb2..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef APM_ADC_h -#define APM_ADC_h - -#define bit_set(p,m) ((p) |= ( 1< // ArduPilot Mega ADC Library - -unsigned long timer; - -void setup() -{ - APM_ADC.Init(); // APM ADC initialization - Serial.begin(57600); - Serial.println("ArduPilot Mega ADC library test"); - delay(1000); - timer = millis(); -} - -void loop() -{ - int ch; - - if((millis()- timer) > 20) - { - timer = millis(); - for (ch=0;ch<7;ch++) - { - Serial.print(APM_ADC.Ch(ch)); - Serial.print(","); - } - Serial.println(); - } -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt deleted file mode 100644 index c82cb5d786..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt +++ /dev/null @@ -1,3 +0,0 @@ -APM_ADC KEYWORD1 -Init KEYWORD2 -Ch KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp deleted file mode 100644 index 72f84b7161..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/* - APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library 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. - - Sensor is conected to I2C port - Sensor End of Conversion (EOC) pin is PC7 (30) - - Variables: - RawTemp : Raw temperature data - RawPress : Raw pressure data - - Temp : Calculated temperature (in 0.1ºC units) - Press : Calculated pressure (in Pa units) - - Methods: - Init() : Initialization of I2C and read sensor calibration data - Read() : Read sensor data and calculate Temperature and Pressure - This function is optimized so the main host don´t need to wait - You can call this function in your main loop - It returns a 1 if there are new data. - - Internal functions: - Command_ReadTemp(): Send commando to read temperature - Command_ReadPress(): Send commando to read Pressure - ReadTemp() : Read temp register - ReadPress() : Read press register - Calculate() : Calculate Temperature and Pressure in real units - - -*/ -extern "C" { - // AVR LibC Includes - #include - #include - #include "WConstants.h" -} - -#include -#include "APM_BMP085.h" - -#define BMP085_ADDRESS 0x77 //(0xEE >> 1) -#define BMP085_EOC 30 // End of conversion pin PC7 - -// Constructors //////////////////////////////////////////////////////////////// -APM_BMP085_Class::APM_BMP085_Class() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void APM_BMP085_Class::Init(void) -{ - unsigned char tmp; - byte buff[22]; - int i=0; - - pinMode(BMP085_EOC,INPUT); // End Of Conversion (PC7) input - Wire.begin(); - oss = 3; // Over Sampling setting 3 = High resolution - BMP085_State = 0; // Initial state - - // We read the calibration data registers - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xAA); - Wire.endTransmission(); - - Wire.requestFrom(BMP085_ADDRESS, 22); - //Wire.endTransmission(); - while(Wire.available()) - { - buff[i] = Wire.receive(); // receive one byte - i++; - } - ac1 = ((int)buff[0] << 8) | buff[1]; - ac2 = ((int)buff[2] << 8) | buff[3]; - ac3 = ((int)buff[4] << 8) | buff[5]; - ac4 = ((int)buff[6] << 8) | buff[7]; - ac5 = ((int)buff[8] << 8) | buff[9]; - ac6 = ((int)buff[10] << 8) | buff[11]; - b1 = ((int)buff[12] << 8) | buff[13]; - b2 = ((int)buff[14] << 8) | buff[15]; - mb = ((int)buff[16] << 8) | buff[17]; - mc = ((int)buff[18] << 8) | buff[19]; - md = ((int)buff[20] << 8) | buff[21]; - - //Send a command to read Temp - Command_ReadTemp(); - BMP085_State=1; -} - - -// Read the sensor. This is a state machine -// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5) -uint8_t APM_BMP085_Class::Read() -{ -uint8_t result=0; - - if (BMP085_State==1) - { - if (digitalRead(BMP085_EOC)) - { - ReadTemp(); // On state 1 we read temp - BMP085_State++; - Command_ReadPress(); - } - } - else - { - if (BMP085_State==5) - { - if (digitalRead(BMP085_EOC)) - { - ReadPress(); - Calculate(); - BMP085_State = 1; // Start again from state=1 - Command_ReadTemp(); // Read Temp - result=1; // New pressure reading - } - } - else - { - if (digitalRead(BMP085_EOC)) - { - ReadPress(); - Calculate(); - BMP085_State++; - Command_ReadPress(); - result=1; // New pressure reading - } - } - } - return(result); -} - - -// Send command to Read Pressure -void APM_BMP085_Class::Command_ReadPress() -{ - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF4); - Wire.send(0x34+(oss<<6)); //write_register(0xF4,0x34+(oversampling_setting<<6)); - Wire.endTransmission(); -} - -// Read Raw Pressure values -void APM_BMP085_Class::ReadPress() -{ - byte msb; - byte lsb; - byte xlsb; - - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF6); - Wire.endTransmission(); - - Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte - while(!Wire.available()) { - // waiting - } - msb = Wire.receive(); - while(!Wire.available()) { - // waiting - } - lsb = Wire.receive(); - while(!Wire.available()) { - // waiting - } - xlsb = Wire.receive(); - RawPress = (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >> (8-oss); -} - -// Send Command to Read Temperature -void APM_BMP085_Class::Command_ReadTemp() -{ - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF4); - Wire.send(0x2E); - Wire.endTransmission(); -} - -// Read Raw Temperature values -void APM_BMP085_Class::ReadTemp() -{ - byte tmp; - - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF6); - Wire.endTransmission(); - - Wire.beginTransmission(BMP085_ADDRESS); - Wire.requestFrom(BMP085_ADDRESS,2); - while(!Wire.available()); // wait - RawTemp = Wire.receive(); - while(!Wire.available()); // wait - tmp = Wire.receive(); - RawTemp = RawTemp<<8 | tmp; -} - -// Calculate Temperature and Pressure in real units. -void APM_BMP085_Class::Calculate() -{ - long x1, x2, x3, b3, b5, b6, p; - unsigned long b4, b7; - int32_t tmp; - - // See Datasheet page 13 for this formulas - // Based also on Jee Labs BMP085 example code. Thanks for share. - // Temperature calculations - x1 = ((long)RawTemp - ac6) * ac5 >> 15; - x2 = ((long) mc << 11) / (x1 + md); - b5 = x1 + x2; - Temp = (b5 + 8) >> 4; - - // Pressure calculations - b6 = b5 - 4000; - x1 = (b2 * (b6 * b6 >> 12)) >> 11; - x2 = ac2 * b6 >> 11; - x3 = x1 + x2; - //b3 = (((int32_t) ac1 * 4 + x3)<> 2; // BAD - //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0 - tmp = ac1; - tmp = (tmp*4 + x3)<> 13; - x2 = (b1 * (b6 * b6 >> 12)) >> 16; - x3 = ((x1 + x2) + 2) >> 2; - b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15; - b7 = ((uint32_t) RawPress - b3) * (50000 >> oss); - p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; - - x1 = (p >> 8) * (p >> 8); - x1 = (x1 * 3038) >> 16; - x2 = (-7357 * p) >> 16; - Press = p + ((x1 + x2 + 3791) >> 4); -} - - -// make one instance for the user to use -APM_BMP085_Class APM_BMP085; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h deleted file mode 100644 index d5ad04932c..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef APM_BMP085_h -#define APM_BMP085_h - - -class APM_BMP085_Class -{ - private: - // State machine - uint8_t BMP085_State; - // Internal calibration registers - int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; - uint16_t ac4, ac5, ac6; - void Command_ReadPress(); - void Command_ReadTemp(); - void ReadPress(); - void ReadTemp(); - void Calculate(); - public: - int32_t RawPress; - int32_t RawTemp; - int16_t Temp; - int32_t Press; - //int Altitude; - uint8_t oss; - //int32_t Press0; // Pressure at sea level - - APM_BMP085_Class(); // Constructor - void Init(); - uint8_t Read(); -}; - -extern APM_BMP085_Class APM_BMP085; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde deleted file mode 100644 index 1ed59977b8..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde +++ /dev/null @@ -1,41 +0,0 @@ -/* - Example of APM_BMP085 (absolute pressure sensor) library. - Code by Jordi Muñoz and Jose Julio. DIYDrones.com -*/ - -#include -#include // ArduPilot Mega BMP085 Library - -unsigned long timer; - -void setup() -{ - APM_BMP085.Init(); // APM ADC initialization - Serial.begin(57600); - Serial.println("ArduPilot Mega BMP085 library test"); - delay(1000); - timer = millis(); -} - -void loop() -{ - int ch; - float tmp_float; - float Altitude; - - if((millis()- timer) > 50) - { - timer=millis(); - APM_BMP085.Read(); - Serial.print("Pressure:"); - Serial.print(APM_BMP085.Press); - Serial.print(" Temperature:"); - Serial.print(APM_BMP085.Temp/10.0); - Serial.print(" Altitude:"); - tmp_float = (APM_BMP085.Press/101325.0); - tmp_float = pow(tmp_float,0.190295); - Altitude = 44330*(1.0-tmp_float); - Serial.print(Altitude); - Serial.println(); - } -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt deleted file mode 100644 index 0d26768c39..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt +++ /dev/null @@ -1,5 +0,0 @@ -APM_BMP085 KEYWORD1 -Init KEYWORD2 -Read KEYWORD2 -Press KEYWORD2 -Temp KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp deleted file mode 100644 index 8d5107a5fc..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp +++ /dev/null @@ -1,206 +0,0 @@ -// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- -// -// Copyright (c) 2010 Michael Smith. All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS -// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY -// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF -// SUCH DAMAGE. - -/// @file BinComm.cpp -/// @brief Implementation of the ArduPilot Mega binary communications -/// library. - -#include "APM_BinComm.h" -#include "WProgram.h" - -/// @name decoder state machine phases -//@{ -#define DEC_WAIT_P1 0 -#define DEC_WAIT_P2 1 -#define DEC_WAIT_HEADER 2 -#define DEC_WAIT_MESSAGE 3 -#define DEC_WAIT_SUM_A 4 -#define DEC_WAIT_SUM_B 5 -//@} - - -/// inter-byte timeout for decode (ms) -#define DEC_MESSAGE_TIMEOUT 100 - -BinComm::BinComm(const BinComm::MessageHandler *handlerTable, - Stream *interface) : - _handlerTable(handlerTable), - _interface(interface) -{ -}; - -void -BinComm::_sendMessage(void) -{ - uint8_t bytesToSend; - uint8_t sumA, sumB; - const uint8_t *p; - - // send the preamble first - _interface->write((uint8_t)MSG_PREAMBLE_1); - _interface->write((uint8_t)MSG_PREAMBLE_2); - - // set up to send the payload - bytesToSend = _encodeBuf.header.length + sizeof(_encodeBuf.header); - sumA = sumB = 0; - p = (const uint8_t *)&_encodeBuf; - - // send message bytes and compute checksum on the fly - while (bytesToSend--) { - sumA += *p; - sumB += sumA; - _interface->write(*p++); - } - - // send the checksum - _interface->write(sumA); - _interface->write(sumB); -} - -void -BinComm::update(void) -{ - uint8_t count; - - // Ensure that we don't spend too long here by only processing - // the bytes that were available when we started. - // - // XXX we might want to further constrain this count - count = _interface->available(); - while (count--) - _decode(_interface->read()); -} - -void -BinComm::_decode(uint8_t inByte) -{ - uint8_t tableIndex; - - // handle inter-byte timeouts (resync after link loss, etc.) - // - if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT) - _decodePhase = DEC_WAIT_P1; - - // run the decode state machine - // - switch (_decodePhase) { - - // Preamble detection - // - // Note the fallthrough from P2 to P1 deals with the case where - // we see 0x34, 0x34, 0x44 where the first 0x34 is garbage or - // a SUM_B byte we never looked at. - // - case DEC_WAIT_P2: - if (MSG_PREAMBLE_2 == inByte) { - _decodePhase++; - - // prepare for the header - _bytesIn = 0; - _bytesExpected = sizeof(MessageHeader); - - // intialise the checksum accumulators - _sumA = _sumB = 0; - - break; - } - _decodePhase = DEC_WAIT_P1; - // FALLTHROUGH - case DEC_WAIT_P1: - if (MSG_PREAMBLE_1 == inByte) { - _decodePhase++; - } - break; - - // receiving the header - // - case DEC_WAIT_HEADER: - // do checksum accumulation - _sumA += inByte; - _sumB += _sumA; - - // store the byte - _decodeBuf.bytes[_bytesIn++] = inByte; - - // check for complete header received - if (_bytesIn == _bytesExpected) { - _decodePhase++; - - // prepare for the payload - // variable-length data? - _bytesIn = 0; - _bytesExpected = _decodeBuf.header.length; - _messageID = _decodeBuf.header.messageID; - _messageVersion = _decodeBuf.header.messageVersion; - - // sanity check to avoid buffer overflow - revert back to waiting - if (_bytesExpected > sizeof(_decodeBuf)) - _decodePhase = DEC_WAIT_P1; - } - break; - - // receiving payload data - // - case DEC_WAIT_MESSAGE: - // do checksum accumulation - _sumA += inByte; - _sumB += _sumA; - - // store the byte - _decodeBuf.bytes[_bytesIn++] = inByte; - - // check for complete payload received - if (_bytesIn == _bytesExpected) { - _decodePhase++; - } - break; - - // waiting for the checksum bytes - // - case DEC_WAIT_SUM_A: - if (inByte != _sumA) { - badMessagesReceived++; - _decodePhase = DEC_WAIT_P1; - } else { - _decodePhase++; - } - break; - case DEC_WAIT_SUM_B: - if (inByte == _sumB) { - // if we got this far, we have a message - messagesReceived++; - - // call any handler interested in this message - for (tableIndex = 0; MSG_NULL != _handlerTable[tableIndex].messageID; tableIndex++) - if ((_handlerTable[tableIndex].messageID == _messageID) || - (_handlerTable[tableIndex].messageID == MSG_ANY)) - _handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg, _messageID, _messageVersion, &_decodeBuf); - } else { - badMessagesReceived++; - } - _decodePhase = DEC_WAIT_P1; - break; - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h deleted file mode 100644 index 22708f2dd7..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h +++ /dev/null @@ -1,267 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// Copyright (c) 2010 Michael Smith. All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS -// OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY -// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF -// SUCH DAMAGE. - -/// @file BinComm.h -/// @brief Definitions for the ArduPilot Mega binary communications -/// library. - -#ifndef APM_BinComm_h -#define APM_BinComm_h - -#include -#include -#include "WProgram.h" - -/// -/// @class BinComm -/// @brief Class providing protocol en/decoding services for the ArduPilot -/// Mega binary telemetry protocol. -/// -class BinComm { -public: - struct MessageHandler; - - ////////////////////////////////////////////////////////////////////// - /// Constructor. - /// - /// @param handlerTable Array of callout functions to which - /// received messages will be sent. More than - /// one handler for a given messageID may be - /// registered; handlers are called in the order - /// they appear in the table. - /// - /// @param interface The stream that will be used - /// for telemetry communications. - /// - BinComm(const MessageHandler *handlerTable, - Stream *interface); - -private: - /// OTA message header - struct MessageHeader { - uint8_t length; - uint8_t messageID; - uint8_t messageVersion; - }; - - /// Incoming header/packet buffer - /// XXX we could make this smaller - union { - uint8_t bytes[0]; - MessageHeader header; - uint8_t payload[256]; - } _decodeBuf; - - /// Outgoing header/packet buffer - /// XXX we could make this smaller - struct { - MessageHeader header; - uint8_t payload[256 - sizeof(MessageHeader)]; - } _encodeBuf; - - - ////////////////////////////////////////////////////////////////////// - /// @name Message pack/unpack utility functions - /// - //@{ - inline void _pack(uint8_t *&ptr, const uint8_t x) { *(uint8_t *)ptr = x; ptr += sizeof(x); } - inline void _pack(uint8_t *&ptr, const uint16_t x) { *(uint16_t *)ptr = x; ptr += sizeof(x); } - inline void _pack(uint8_t *&ptr, const int16_t x) { *(int16_t *)ptr = x; ptr += sizeof(x); } - inline void _pack(uint8_t *&ptr, const uint32_t x) { *(uint32_t *)ptr = x; ptr += sizeof(x); } - inline void _pack(uint8_t *&ptr, const int32_t x) { *(int32_t *)ptr = x; ptr += sizeof(x); } - - inline void _pack(uint8_t *&ptr, const char *msg, uint8_t size) { strlcpy((char *)ptr, msg, size); ptr += size; } - inline void _pack(uint8_t *&ptr, const uint8_t *values, uint8_t count) { memcpy(ptr, values, count); ptr += count; } - inline void _pack(uint8_t *&ptr, const uint16_t *values, uint8_t count) { memcpy(ptr, values, count * 2); ptr += count * 2; } - - inline void _unpack(uint8_t *&ptr, uint8_t &x) { x = *(uint8_t *)ptr; ptr += sizeof(x); } - inline void _unpack(uint8_t *&ptr, uint16_t &x) { x = *(uint16_t *)ptr; ptr += sizeof(x); } - inline void _unpack(uint8_t *&ptr, int16_t &x) { x = *(int16_t *)ptr; ptr += sizeof(x); } - inline void _unpack(uint8_t *&ptr, uint32_t &x) { x = *(uint32_t *)ptr; ptr += sizeof(x); } - inline void _unpack(uint8_t *&ptr, int32_t &x) { x = *(int32_t *)ptr; ptr += sizeof(x); } - - inline void _unpack(uint8_t *&ptr, char *msg, uint8_t size) { strlcpy(msg, (char *)ptr, size); ptr += size; } - inline void _unpack(uint8_t *&ptr, uint8_t *values, uint8_t count) { memcpy(values, ptr, count); ptr += count; } - inline void _unpack(uint8_t *&ptr, uint16_t *values, uint8_t count) { memcpy(values, ptr, count * 2); ptr += count * 2; } - //@} - -public: - ////////////////////////////////////////////////////////////////////// - /// @name Protocol definition - /// - /// The protocol definition, including structures describing messages, - /// MessageID values and helper functions for packing messages are - /// automatically generated. - //@{ -#include "protocol/protocol.h" - //@} - - ////////////////////////////////////////////////////////////////////// - /// @name Protocol magic numbers - /// - /// @note The MessageID enum is automatically generated and thus not described here. - /// - //@{ - - /// Variables defined - /// XXX these should probably be handled by the database/MIB? - enum variableID { - MSG_VAR_ROLL_MODE = 0x00, - MSG_VAR_PITCH_MODE = 0x01, - MSG_VAR_THROTTLE_MODE = 0x02, - MSG_VAR_YAW_MODE = 0x03, - MSG_VAR_ELEVON_TRIM_1 = 0x04, - MSG_VAR_ELEVON_TRIM_2 = 0x05, - - MSG_VAR_INTEGRATOR_0 = 0x10, - MSG_VAR_INTEGRATOR_1 = 0x11, - MSG_VAR_INTEGRATOR_2 = 0x12, - MSG_VAR_INTEGRATOR_3 = 0x13, - MSG_VAR_INTEGRATOR_4 = 0x14, - MSG_VAR_INTEGRATOR_5 = 0x15, - MSG_VAR_INTEGRATOR_6 = 0x16, - MSG_VAR_INTEGRATOR_7 = 0x17, - - MSG_VAR_KFF_0 = 0x1a, - MSG_VAR_KFF_1 = 0x1b, - MSG_VAR_KFF_2 = 0x1c, - - MSG_VAR_TARGET_BEARING = 0x20, - MSG_VAR_NAV_BEARING = 0x21, - MSG_VAR_BEARING_ERROR = 0x22, - MSG_VAR_CROSSTRACK_BEARING = 0x23, - MSG_VAR_CROSSTRACK_ERROR = 0x24, - MSG_VAR_ALTITUDE_ERROR = 0x25, - MSG_VAR_WP_RADIUS = 0x26, - MSG_VAR_LOITER_RADIUS = 0x27, - MSG_VAR_WP_MODE = 0x28, - MSG_VAR_LOOP_COMMANDS = 0x29, - MSG_VAR_NAV_GAIN_SCALER = 0x2a, - }; - - /// PID sets defined - enum PIDSet { - MSG_SERVO_ROLL = 0, - MSG_SERVO_PITCH = 1, - MSG_SERVO_RUDDER = 2, - MSG_SERVO_NAV_ROLL = 3, - MSG_SERVO_NAV_PITCH_ASP = 4, - MSG_SERVO_NAV_PITCH_ALT = 5, - MSG_SERVO_TE_THROTTLE = 6, - MSG_SERVO_ALT_THROTTLE = 7, - MSG_SERVO_ELEVATOR = 8 // Added by Randy - }; - - //@} - - ////////////////////////////////////////////////////////////////////// - /// Message reception callout descriptor - /// - /// An array of these handlers is passed to the constructor to delegate - /// processing for received messages. - /// - struct MessageHandler { - MessageID messageID; ///< messageID for which the handler will be called - void (* handler)(void *arg, - uint8_t messageId, - uint8_t messageVersion, - void *messageData); ///< function to be called - void *arg; ///< argument passed to function - }; - - ////////////////////////////////////////////////////////////////////// - /// @name Decoder interface - //@{ - - /// Consume bytes from the interface and feed them to the decoder. - /// - /// If a packet is completed, then any callbacks associated - /// with the packet's messageID will be called. - /// - /// If no bytes are passed to the decoder for a period determined - /// by DEC_MESSAGE_TIMEOUT, the decode state machine will reset - /// before processing the next byte. This can help re-synchronise - /// after a link loss or in-flight failure. - /// - - void update(void); - - uint32_t messagesReceived; ///< statistics - uint32_t badMessagesReceived; ///< statistics - - //@} - - ////////////////////////////////////////////////////////////////////// - /// @name Encoder interface - /// - /// Messages are normally encoded and sent using the - /// send_msg_* functions defined in protocol/protocol.h. - /// For each message type MSG_* there is a corresponding send_msg_* - /// function which will construct and transmit the message. - /// - //@{ - uint32_t messagesSent; ///< statistics - //@} - - -private: - const MessageHandler *_handlerTable; ///< callout table - Stream *_interface; ///< Serial port we send/receive using. - - /// Various magic numbers - enum MagicNumbers { - MSG_PREAMBLE_1 = 0x34, - MSG_PREAMBLE_2 = 0x44, - MSG_VERSION_1 = 1, - MSG_VARIABLE_LENGTH = 0xff - }; - - ////////////////////////////////////////////////////////////////////// - /// @name Decoder state - //@{ - uint8_t _decodePhase; ///< decoder state machine phase - uint8_t _bytesIn; ///< bytes received in the current phase - uint8_t _bytesExpected; ///< bytes expected in the current phase - uint8_t _sumA; ///< sum of incoming bytes - uint8_t _sumB; ///< sum of _sumA values - - uint8_t _messageID; ///< messageID from the packet being received - uint8_t _messageVersion;///< messageVersion from the packet being received - - unsigned long _lastReceived; ///< timestamp of last byte reception - //@} - - /// Decoder state machine. - /// - /// @param inByte The byte to process. - /// - void _decode(uint8_t inByte); - - /// Send the packet in the encode buffer. - /// - void _sendMessage(void); -}; - -#endif // BinComm_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt deleted file mode 100644 index 3d75c5372d..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt +++ /dev/null @@ -1,6 +0,0 @@ -APM_BinComm KEYWORD1 -update KEYWORD2 -messagesReceived KEYWORD2 -badMessagesReceived KEYWORD2 -messagesSent KEYWORD2 - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def deleted file mode 100644 index d1e465cc6f..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def +++ /dev/null @@ -1,224 +0,0 @@ -# -# Message definitions for the ArduPilot Mega binary communications protocol. -# -# Process this file using: -# -# awk -f protogen.awk protocol.def > protocol.h -# -# Messages are declared with -# -# message -# -# is a valid member of BinComm::MessageID. -# is the message ID byte -# -# Following a message declaration the fields of the message are -# defined in the format: -# -# [] -# -# is a C type corresponding to the field. The type must be a single -# word, either an integer from or "char". -# is the name of the field; it should be unique within the message -# is an optional array count for fields that are arrays -# - -# -# Acknowledge message -# -message 0x00 MSG_ACKNOWLEDGE - uint8_t msgID - uint16_t msgSum - -# -# System heartbeat -# -message 0x01 MSG_HEARTBEAT - uint8_t flightMode - uint16_t timeStamp - uint16_t batteryVoltage - uint16_t commandIndex - -# -# Attitude report -# -message 0x02 MSG_ATTITUDE - int16_t roll - int16_t pitch - int16_t yaw - -# -# Location report -# -message 0x03 MSG_LOCATION - int32_t latitude - int32_t longitude - int16_t altitude - int16_t groundSpeed - int16_t groundCourse - uint16_t timeOfWeek - -# -# Optional pressure-based location report -# -message 0x04 MSG_PRESSURE - uint16_t pressureAltitude - uint16_t airSpeed - -# -# Text status message -# -message 0x05 MSG_STATUS_TEXT - uint8_t severity - char text 50 - -# -# Algorithm performance report -# -message 0x06 MSG_PERF_REPORT - uint32_t interval - uint16_t mainLoopCycles - uint8_t mainLoopTime - uint8_t gyroSaturationCount - uint8_t adcConstraintCount - uint16_t imuHealth - uint16_t gcsMessageCount - -# -# System version messages -# -message 0x07 MSG_VERSION_REQUEST - uint8_t systemType - uint8_t systemID - -message 0x08 MSG_VERSION - uint8_t systemType - uint8_t systemID - uint8_t firmwareVersion 3 - -# -# Flight command operations -# -message 0x20 MSG_COMMAND_REQUEST - uint16_t UNSPECIFIED - -message 0x21 MSG_COMMAND_UPLOAD - uint8_t action - uint16_t itemNumber - int listLength - uint8_t commandID - uint8_t p1 - uint16_t p2 - uint32_t p3 - uint32_t p4 - -message 0x22 MSG_COMMAND_LIST - int itemNumber - int listLength - uint8_t commandID - uint8_t p1 - uint16_t p2 - uint32_t p3 - uint32_t p4 - -message 0x23 MSG_COMMAND_MODE_CHANGE - uint16_t UNSPECIFIED - -# -# Parameter operations -# -message 0x30 MSG_VALUE_REQUEST - uint8_t valueID - uint8_t broadcast - - -message 0x31 MSG_VALUE_SET - uint8_t valueID - uint32_t value - -message 0x32 MSG_VALUE - uint8_t valueID - uint32_t value - -# -# PID adjustments -# -message 0x40 MSG_PID_REQUEST - uint8_t pidSet - -message 0x41 MSG_PID_SET - uint8_t pidSet - int32_t p - int32_t i - int32_t d - int16_t integratorMax - -message 0x42 MSG_PID - uint8_t pidSet - int32_t p - int32_t i - int32_t d - int16_t integratorMax - - -# -# Radio settings and values -# -message 0x50 MSG_TRIM_STARTUP - uint16_t value 8 - -message 0x51 MSG_TRIM_MIN - uint16_t value 8 - -message 0x52 MSG_TRIM_MAX - uint16_t value 8 - -message 0x53 MSG_SERVOS - int16_t ch1 - int16_t ch2 - int16_t ch3 - int16_t ch4 - int16_t ch5 - int16_t ch6 - int16_t ch7 - int16_t ch8 - -# -# Direct sensor access -# -message 0x60 MSG_SENSOR - uint16_t UNSPECIFIED - -# -# Simulation-related messages -# -message 0x70 MSG_SIM - uint16_t UNSPECIFIED - -# -# Direct I/O pin control -# -message 0x80 MSG_PIN_REQUEST - uint16_t UNSPECIFIED - -message 0x81 MSG_PIN_SET - uint16_t UNSPECIFIED - -# -# Dataflash operations -# -message 0x90 MSG_DATAFLASH_REQUEST - uint16_t UNSPECIFIED - -message 0x91 MSG_DATAFLASH_SET - uint16_t UNSPECIFIED - -# -# EEPROM operations -# -message 0xa0 MSG_EEPROM_REQUEST - uint16_t UNSPECIFIED - -message 0xa1 MSG_EEPROM_SET - uint16_t UNSPECIFIED - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h deleted file mode 100644 index 2da8e0f7c3..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h +++ /dev/null @@ -1,1301 +0,0 @@ -// -// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT -// -/// @file protocol.h -#pragma pack(1) - -////////////////////////////////////////////////////////////////////// -/// @name MSG_ACKNOWLEDGE -//@{ - -/// Structure describing the payload section of the MSG_ACKNOWLEDGE message -struct msg_acknowledge { - uint8_t msgID; - uint16_t msgSum; -}; - -/// Send a MSG_ACKNOWLEDGE message -inline void -send_msg_acknowledge( - const uint8_t msgID, - const uint16_t msgSum) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, msgID); - _pack(__p, msgSum); - _encodeBuf.header.length = 3; - _encodeBuf.header.messageID = MSG_ACKNOWLEDGE; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_ACKNOWLEDGE message -inline void -unpack_msg_acknowledge( - uint8_t &msgID, - uint16_t &msgSum) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, msgID); - _unpack(__p, msgSum); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_HEARTBEAT -//@{ - -/// Structure describing the payload section of the MSG_HEARTBEAT message -struct msg_heartbeat { - uint8_t flightMode; - uint16_t timeStamp; - uint16_t batteryVoltage; - uint16_t commandIndex; -}; - -/// Send a MSG_HEARTBEAT message -inline void -send_msg_heartbeat( - const uint8_t flightMode, - const uint16_t timeStamp, - const uint16_t batteryVoltage, - const uint16_t commandIndex) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, flightMode); - _pack(__p, timeStamp); - _pack(__p, batteryVoltage); - _pack(__p, commandIndex); - _encodeBuf.header.length = 7; - _encodeBuf.header.messageID = MSG_HEARTBEAT; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_HEARTBEAT message -inline void -unpack_msg_heartbeat( - uint8_t &flightMode, - uint16_t &timeStamp, - uint16_t &batteryVoltage, - uint16_t &commandIndex) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, flightMode); - _unpack(__p, timeStamp); - _unpack(__p, batteryVoltage); - _unpack(__p, commandIndex); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_ATTITUDE -//@{ - -/// Structure describing the payload section of the MSG_ATTITUDE message -struct msg_attitude { - int16_t roll; - int16_t pitch; - int16_t yaw; -}; - -/// Send a MSG_ATTITUDE message -inline void -send_msg_attitude( - const int16_t roll, - const int16_t pitch, - const int16_t yaw) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, roll); - _pack(__p, pitch); - _pack(__p, yaw); - _encodeBuf.header.length = 6; - _encodeBuf.header.messageID = MSG_ATTITUDE; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_ATTITUDE message -inline void -unpack_msg_attitude( - int16_t &roll, - int16_t &pitch, - int16_t &yaw) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, roll); - _unpack(__p, pitch); - _unpack(__p, yaw); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_LOCATION -//@{ - -/// Structure describing the payload section of the MSG_LOCATION message -struct msg_location { - int32_t latitude; - int32_t longitude; - int16_t altitude; - int16_t groundSpeed; - int16_t groundCourse; - uint16_t timeOfWeek; -}; - -/// Send a MSG_LOCATION message -inline void -send_msg_location( - const int32_t latitude, - const int32_t longitude, - const int16_t altitude, - const int16_t groundSpeed, - const int16_t groundCourse, - const uint16_t timeOfWeek) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, latitude); - _pack(__p, longitude); - _pack(__p, altitude); - _pack(__p, groundSpeed); - _pack(__p, groundCourse); - _pack(__p, timeOfWeek); - _encodeBuf.header.length = 16; - _encodeBuf.header.messageID = MSG_LOCATION; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_LOCATION message -inline void -unpack_msg_location( - int32_t &latitude, - int32_t &longitude, - int16_t &altitude, - int16_t &groundSpeed, - int16_t &groundCourse, - uint16_t &timeOfWeek) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, latitude); - _unpack(__p, longitude); - _unpack(__p, altitude); - _unpack(__p, groundSpeed); - _unpack(__p, groundCourse); - _unpack(__p, timeOfWeek); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PRESSURE -//@{ - -/// Structure describing the payload section of the MSG_PRESSURE message -struct msg_pressure { - uint16_t pressureAltitude; - uint16_t airSpeed; -}; - -/// Send a MSG_PRESSURE message -inline void -send_msg_pressure( - const uint16_t pressureAltitude, - const uint16_t airSpeed) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, pressureAltitude); - _pack(__p, airSpeed); - _encodeBuf.header.length = 4; - _encodeBuf.header.messageID = MSG_PRESSURE; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_PRESSURE message -inline void -unpack_msg_pressure( - uint16_t &pressureAltitude, - uint16_t &airSpeed) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pressureAltitude); - _unpack(__p, airSpeed); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_STATUS_TEXT -//@{ - -/// Structure describing the payload section of the MSG_STATUS_TEXT message -struct msg_status_text { - uint8_t severity; - char text[50]; -}; - -/// Send a MSG_STATUS_TEXT message -inline void -send_msg_status_text( - const uint8_t severity, - const char (&text)[50]) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, severity); - _pack(__p, text, 50); - _encodeBuf.header.length = 51; - _encodeBuf.header.messageID = MSG_STATUS_TEXT; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_STATUS_TEXT message -inline void -unpack_msg_status_text( - uint8_t &severity, - char (&text)[50]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, severity); - _unpack(__p, text, 50); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PERF_REPORT -//@{ - -/// Structure describing the payload section of the MSG_PERF_REPORT message -struct msg_perf_report { - uint32_t interval; - uint16_t mainLoopCycles; - uint8_t mainLoopTime; - uint8_t gyroSaturationCount; - uint8_t adcConstraintCount; - uint16_t imuHealth; - uint16_t gcsMessageCount; -}; - -/// Send a MSG_PERF_REPORT message -inline void -send_msg_perf_report( - const uint32_t interval, - const uint16_t mainLoopCycles, - const uint8_t mainLoopTime, - const uint8_t gyroSaturationCount, - const uint8_t adcConstraintCount, - const uint16_t imuHealth, - const uint16_t gcsMessageCount) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, interval); - _pack(__p, mainLoopCycles); - _pack(__p, mainLoopTime); - _pack(__p, gyroSaturationCount); - _pack(__p, adcConstraintCount); - _pack(__p, imuHealth); - _pack(__p, gcsMessageCount); - _encodeBuf.header.length = 13; - _encodeBuf.header.messageID = MSG_PERF_REPORT; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_PERF_REPORT message -inline void -unpack_msg_perf_report( - uint32_t &interval, - uint16_t &mainLoopCycles, - uint8_t &mainLoopTime, - uint8_t &gyroSaturationCount, - uint8_t &adcConstraintCount, - uint16_t &imuHealth, - uint16_t &gcsMessageCount) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, interval); - _unpack(__p, mainLoopCycles); - _unpack(__p, mainLoopTime); - _unpack(__p, gyroSaturationCount); - _unpack(__p, adcConstraintCount); - _unpack(__p, imuHealth); - _unpack(__p, gcsMessageCount); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VERSION_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_VERSION_REQUEST message -struct msg_version_request { - uint8_t systemType; - uint8_t systemID; -}; - -/// Send a MSG_VERSION_REQUEST message -inline void -send_msg_version_request( - const uint8_t systemType, - const uint8_t systemID) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, systemType); - _pack(__p, systemID); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_VERSION_REQUEST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_VERSION_REQUEST message -inline void -unpack_msg_version_request( - uint8_t &systemType, - uint8_t &systemID) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, systemType); - _unpack(__p, systemID); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VERSION -//@{ - -/// Structure describing the payload section of the MSG_VERSION message -struct msg_version { - uint8_t systemType; - uint8_t systemID; - uint8_t firmwareVersion[3]; -}; - -/// Send a MSG_VERSION message -inline void -send_msg_version( - const uint8_t systemType, - const uint8_t systemID, - const uint8_t (&firmwareVersion)[3]) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, systemType); - _pack(__p, systemID); - _pack(__p, firmwareVersion, 3); - _encodeBuf.header.length = 5; - _encodeBuf.header.messageID = MSG_VERSION; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_VERSION message -inline void -unpack_msg_version( - uint8_t &systemType, - uint8_t &systemID, - uint8_t (&firmwareVersion)[3]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, systemType); - _unpack(__p, systemID); - _unpack(__p, firmwareVersion, 3); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_REQUEST message -struct msg_command_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_COMMAND_REQUEST message -inline void -send_msg_command_request( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_COMMAND_REQUEST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_COMMAND_REQUEST message -inline void -unpack_msg_command_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_UPLOAD -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_UPLOAD message -struct msg_command_upload { - uint8_t action; - uint16_t itemNumber; - int listLength; - uint8_t commandID; - uint8_t p1; - uint16_t p2; - uint32_t p3; - uint32_t p4; -}; - -/// Send a MSG_COMMAND_UPLOAD message -inline void -send_msg_command_upload( - const uint8_t action, - const uint16_t itemNumber, - const int listLength, - const uint8_t commandID, - const uint8_t p1, - const uint16_t p2, - const uint32_t p3, - const uint32_t p4) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, action); - _pack(__p, itemNumber); - _pack(__p, listLength); - _pack(__p, commandID); - _pack(__p, p1); - _pack(__p, p2); - _pack(__p, p3); - _pack(__p, p4); - _encodeBuf.header.length = 16; - _encodeBuf.header.messageID = MSG_COMMAND_UPLOAD; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_COMMAND_UPLOAD message -inline void -unpack_msg_command_upload( - uint8_t &action, - uint16_t &itemNumber, - int &listLength, - uint8_t &commandID, - uint8_t &p1, - uint16_t &p2, - uint32_t &p3, - uint32_t &p4) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, action); - _unpack(__p, itemNumber); - _unpack(__p, listLength); - _unpack(__p, commandID); - _unpack(__p, p1); - _unpack(__p, p2); - _unpack(__p, p3); - _unpack(__p, p4); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_LIST -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_LIST message -struct msg_command_list { - int itemNumber; - int listLength; - uint8_t commandID; - uint8_t p1; - uint16_t p2; - uint32_t p3; - uint32_t p4; -}; - -/// Send a MSG_COMMAND_LIST message -inline void -send_msg_command_list( - const int itemNumber, - const int listLength, - const uint8_t commandID, - const uint8_t p1, - const uint16_t p2, - const uint32_t p3, - const uint32_t p4) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, itemNumber); - _pack(__p, listLength); - _pack(__p, commandID); - _pack(__p, p1); - _pack(__p, p2); - _pack(__p, p3); - _pack(__p, p4); - _encodeBuf.header.length = 14; - _encodeBuf.header.messageID = MSG_COMMAND_LIST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_COMMAND_LIST message -inline void -unpack_msg_command_list( - int &itemNumber, - int &listLength, - uint8_t &commandID, - uint8_t &p1, - uint16_t &p2, - uint32_t &p3, - uint32_t &p4) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, itemNumber); - _unpack(__p, listLength); - _unpack(__p, commandID); - _unpack(__p, p1); - _unpack(__p, p2); - _unpack(__p, p3); - _unpack(__p, p4); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_COMMAND_MODE_CHANGE -//@{ - -/// Structure describing the payload section of the MSG_COMMAND_MODE_CHANGE message -struct msg_command_mode_change { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_COMMAND_MODE_CHANGE message -inline void -send_msg_command_mode_change( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_COMMAND_MODE_CHANGE; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_COMMAND_MODE_CHANGE message -inline void -unpack_msg_command_mode_change( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VALUE_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_VALUE_REQUEST message -struct msg_value_request { - uint8_t valueID; - uint8_t broadcast; -}; - -/// Send a MSG_VALUE_REQUEST message -inline void -send_msg_value_request( - const uint8_t valueID, - const uint8_t broadcast) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, valueID); - _pack(__p, broadcast); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_VALUE_REQUEST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_VALUE_REQUEST message -inline void -unpack_msg_value_request( - uint8_t &valueID, - uint8_t &broadcast) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, valueID); - _unpack(__p, broadcast); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VALUE_SET -//@{ - -/// Structure describing the payload section of the MSG_VALUE_SET message -struct msg_value_set { - uint8_t valueID; - uint32_t value; -}; - -/// Send a MSG_VALUE_SET message -inline void -send_msg_value_set( - const uint8_t valueID, - const uint32_t value) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, valueID); - _pack(__p, value); - _encodeBuf.header.length = 5; - _encodeBuf.header.messageID = MSG_VALUE_SET; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_VALUE_SET message -inline void -unpack_msg_value_set( - uint8_t &valueID, - uint32_t &value) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, valueID); - _unpack(__p, value); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_VALUE -//@{ - -/// Structure describing the payload section of the MSG_VALUE message -struct msg_value { - uint8_t valueID; - uint32_t value; -}; - -/// Send a MSG_VALUE message -inline void -send_msg_value( - const uint8_t valueID, - const uint32_t value) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, valueID); - _pack(__p, value); - _encodeBuf.header.length = 5; - _encodeBuf.header.messageID = MSG_VALUE; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_VALUE message -inline void -unpack_msg_value( - uint8_t &valueID, - uint32_t &value) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, valueID); - _unpack(__p, value); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PID_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_PID_REQUEST message -struct msg_pid_request { - uint8_t pidSet; -}; - -/// Send a MSG_PID_REQUEST message -inline void -send_msg_pid_request( - const uint8_t pidSet) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, pidSet); - _encodeBuf.header.length = 1; - _encodeBuf.header.messageID = MSG_PID_REQUEST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_PID_REQUEST message -inline void -unpack_msg_pid_request( - uint8_t &pidSet) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pidSet); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PID_SET -//@{ - -/// Structure describing the payload section of the MSG_PID_SET message -struct msg_pid_set { - uint8_t pidSet; - int32_t p; - int32_t i; - int32_t d; - int16_t integratorMax; -}; - -/// Send a MSG_PID_SET message -inline void -send_msg_pid_set( - const uint8_t pidSet, - const int32_t p, - const int32_t i, - const int32_t d, - const int16_t integratorMax) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, pidSet); - _pack(__p, p); - _pack(__p, i); - _pack(__p, d); - _pack(__p, integratorMax); - _encodeBuf.header.length = 15; - _encodeBuf.header.messageID = MSG_PID_SET; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_PID_SET message -inline void -unpack_msg_pid_set( - uint8_t &pidSet, - int32_t &p, - int32_t &i, - int32_t &d, - int16_t &integratorMax) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pidSet); - _unpack(__p, p); - _unpack(__p, i); - _unpack(__p, d); - _unpack(__p, integratorMax); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PID -//@{ - -/// Structure describing the payload section of the MSG_PID message -struct msg_pid { - uint8_t pidSet; - int32_t p; - int32_t i; - int32_t d; - int16_t integratorMax; -}; - -/// Send a MSG_PID message -inline void -send_msg_pid( - const uint8_t pidSet, - const int32_t p, - const int32_t i, - const int32_t d, - const int16_t integratorMax) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, pidSet); - _pack(__p, p); - _pack(__p, i); - _pack(__p, d); - _pack(__p, integratorMax); - _encodeBuf.header.length = 15; - _encodeBuf.header.messageID = MSG_PID; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_PID message -inline void -unpack_msg_pid( - uint8_t &pidSet, - int32_t &p, - int32_t &i, - int32_t &d, - int16_t &integratorMax) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, pidSet); - _unpack(__p, p); - _unpack(__p, i); - _unpack(__p, d); - _unpack(__p, integratorMax); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_TRIM_STARTUP -//@{ - -/// Structure describing the payload section of the MSG_TRIM_STARTUP message -struct msg_trim_startup { - uint16_t value[8]; -}; - -/// Send a MSG_TRIM_STARTUP message -inline void -send_msg_trim_startup( - const uint16_t (&value)[8]) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, value, 8); - _encodeBuf.header.length = 16; - _encodeBuf.header.messageID = MSG_TRIM_STARTUP; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_TRIM_STARTUP message -inline void -unpack_msg_trim_startup( - uint16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_TRIM_MIN -//@{ - -/// Structure describing the payload section of the MSG_TRIM_MIN message -struct msg_trim_min { - uint16_t value[8]; -}; - -/// Send a MSG_TRIM_MIN message -inline void -send_msg_trim_min( - const uint16_t (&value)[8]) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, value, 8); - _encodeBuf.header.length = 16; - _encodeBuf.header.messageID = MSG_TRIM_MIN; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_TRIM_MIN message -inline void -unpack_msg_trim_min( - uint16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_TRIM_MAX -//@{ - -/// Structure describing the payload section of the MSG_TRIM_MAX message -struct msg_trim_max { - uint16_t value[8]; -}; - -/// Send a MSG_TRIM_MAX message -inline void -send_msg_trim_max( - const uint16_t (&value)[8]) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, value, 8); - _encodeBuf.header.length = 16; - _encodeBuf.header.messageID = MSG_TRIM_MAX; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_TRIM_MAX message -inline void -unpack_msg_trim_max( - uint16_t (&value)[8]) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, value, 8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_SERVOS -//@{ - -/// Structure describing the payload section of the MSG_SERVOS message -struct msg_servos { - int16_t ch1; - int16_t ch2; - int16_t ch3; - int16_t ch4; - int16_t ch5; - int16_t ch6; - int16_t ch7; - int16_t ch8; -}; - -/// Send a MSG_SERVOS message -inline void -send_msg_servos( - const int16_t ch1, - const int16_t ch2, - const int16_t ch3, - const int16_t ch4, - const int16_t ch5, - const int16_t ch6, - const int16_t ch7, - const int16_t ch8) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, ch1); - _pack(__p, ch2); - _pack(__p, ch3); - _pack(__p, ch4); - _pack(__p, ch5); - _pack(__p, ch6); - _pack(__p, ch7); - _pack(__p, ch8); - _encodeBuf.header.length = 16; - _encodeBuf.header.messageID = MSG_SERVOS; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_SERVOS message -inline void -unpack_msg_servos( - int16_t &ch1, - int16_t &ch2, - int16_t &ch3, - int16_t &ch4, - int16_t &ch5, - int16_t &ch6, - int16_t &ch7, - int16_t &ch8) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, ch1); - _unpack(__p, ch2); - _unpack(__p, ch3); - _unpack(__p, ch4); - _unpack(__p, ch5); - _unpack(__p, ch6); - _unpack(__p, ch7); - _unpack(__p, ch8); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_SENSOR -//@{ - -/// Structure describing the payload section of the MSG_SENSOR message -struct msg_sensor { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_SENSOR message -inline void -send_msg_sensor( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_SENSOR; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_SENSOR message -inline void -unpack_msg_sensor( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_SIM -//@{ - -/// Structure describing the payload section of the MSG_SIM message -struct msg_sim { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_SIM message -inline void -send_msg_sim( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_SIM; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_SIM message -inline void -unpack_msg_sim( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PIN_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_PIN_REQUEST message -struct msg_pin_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_PIN_REQUEST message -inline void -send_msg_pin_request( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_PIN_REQUEST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_PIN_REQUEST message -inline void -unpack_msg_pin_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_PIN_SET -//@{ - -/// Structure describing the payload section of the MSG_PIN_SET message -struct msg_pin_set { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_PIN_SET message -inline void -send_msg_pin_set( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_PIN_SET; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_PIN_SET message -inline void -unpack_msg_pin_set( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_DATAFLASH_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_DATAFLASH_REQUEST message -struct msg_dataflash_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_DATAFLASH_REQUEST message -inline void -send_msg_dataflash_request( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_DATAFLASH_REQUEST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_DATAFLASH_REQUEST message -inline void -unpack_msg_dataflash_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_DATAFLASH_SET -//@{ - -/// Structure describing the payload section of the MSG_DATAFLASH_SET message -struct msg_dataflash_set { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_DATAFLASH_SET message -inline void -send_msg_dataflash_set( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_DATAFLASH_SET; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_DATAFLASH_SET message -inline void -unpack_msg_dataflash_set( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_EEPROM_REQUEST -//@{ - -/// Structure describing the payload section of the MSG_EEPROM_REQUEST message -struct msg_eeprom_request { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_EEPROM_REQUEST message -inline void -send_msg_eeprom_request( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_EEPROM_REQUEST; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_EEPROM_REQUEST message -inline void -unpack_msg_eeprom_request( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// @name MSG_EEPROM_SET -//@{ - -/// Structure describing the payload section of the MSG_EEPROM_SET message -struct msg_eeprom_set { - uint16_t UNSPECIFIED; -}; - -/// Send a MSG_EEPROM_SET message -inline void -send_msg_eeprom_set( - const uint16_t UNSPECIFIED) -{ - uint8_t *__p = &_encodeBuf.payload[0]; - _pack(__p, UNSPECIFIED); - _encodeBuf.header.length = 2; - _encodeBuf.header.messageID = MSG_EEPROM_SET; - _encodeBuf.header.messageVersion = MSG_VERSION_1; - _sendMessage(); -}; - -/// Unpack a MSG_EEPROM_SET message -inline void -unpack_msg_eeprom_set( - uint16_t &UNSPECIFIED) -{ - uint8_t *__p = &_decodeBuf.payload[0]; - _unpack(__p, UNSPECIFIED); -}; -//@} - -////////////////////////////////////////////////////////////////////// -/// Message ID values -enum MessageID { - MSG_PID = 0x42, - MSG_DATAFLASH_REQUEST = 0x90, - MSG_DATAFLASH_SET = 0x91, - MSG_SENSOR = 0x60, - MSG_VALUE_REQUEST = 0x30, - MSG_VALUE_SET = 0x31, - MSG_VALUE = 0x32, - MSG_PIN_REQUEST = 0x80, - MSG_PIN_SET = 0x81, - MSG_ACKNOWLEDGE = 0x0, - MSG_HEARTBEAT = 0x1, - MSG_ATTITUDE = 0x2, - MSG_LOCATION = 0x3, - MSG_PRESSURE = 0x4, - MSG_TRIM_STARTUP = 0x50, - MSG_STATUS_TEXT = 0x5, - MSG_TRIM_MIN = 0x51, - MSG_PERF_REPORT = 0x6, - MSG_TRIM_MAX = 0x52, - MSG_VERSION_REQUEST = 0x7, - MSG_SERVOS = 0x53, - MSG_VERSION = 0x8, - MSG_COMMAND_REQUEST = 0x20, - MSG_COMMAND_UPLOAD = 0x21, - MSG_COMMAND_LIST = 0x22, - MSG_COMMAND_MODE_CHANGE = 0x23, - MSG_SIM = 0x70, - MSG_EEPROM_REQUEST = 0xa0, - MSG_EEPROM_SET = 0xa1, - MSG_PID_REQUEST = 0x40, - MSG_PID_SET = 0x41, - MSG_ANY = 0xfe, - MSG_NULL = 0xff -}; -#pragma pack(pop) diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk deleted file mode 100644 index 3b5e0b65e4..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk +++ /dev/null @@ -1,174 +0,0 @@ -# -# Process the protocol specification and emit functions to pack and unpack buffers. -# -# See protocol.def for a description of the definition format. -# - -BEGIN { - printf("//\n// THIS FILE WAS AUTOMATICALLY GENERATED - DO NOT EDIT\n//\n") - printf("/// @file protocol.h\n") - printf("#pragma pack(1)\n"); - - currentMessage = "" -} - -END { - # finalise the last message definition - EMIT_MESSAGE() - - # - # emit the MessageID enum - # - # XXX it would be elegant to sort the array here, but not - # everyone has GNU awk. - # - printf("\n//////////////////////////////////////////////////////////////////////\n") - printf("/// Message ID values\n") - printf("enum MessageID {\n") - for (opcode in opcodes) { - printf("\t%s = 0x%x,\n", opcodes[opcode], opcode) - } - printf("\tMSG_ANY = 0xfe,\n") - printf("\tMSG_NULL = 0xff\n") - printf("};\n") - - printf("#pragma pack(pop)\n") -} - -# -# Emit definitions for one message -# -function EMIT_MESSAGE(payloadSize) -{ - if (currentMessage != "") { - printf("\n//////////////////////////////////////////////////////////////////////\n") - printf("/// @name %s \n//@{\n\n", currentMessage) - - # - # emit a structure defining the message payload - # - printf("/// Structure describing the payload section of the %s message\n", currentMessage) - printf("struct %s {\n", tolower(currentMessage)) - for (i = 0; i < fieldCount; i++) { - printf("\t%s %s", types[i], names[i]) - if (counts[i]) - printf("[%s]", counts[i]) - printf(";\n") - } - printf("};\n\n") - - # - # emit a routine to pack the message payload from a set of variables and send it - # - printf("/// Send a %s message\n", currentMessage) - printf("inline void\nsend_%s(\n", tolower(currentMessage)) - for (i = 0; i < fieldCount; i++) { - if (counts[i]) { - printf("\tconst %s (&%s)[%d]", types[i], names[i], counts[i]) - } else { - printf("\tconst %s %s", types[i], names[i]) - } - if (i < (fieldCount -1)) - printf(",\n"); - } - printf(")\n{\n") - printf("\tuint8_t *__p = &_encodeBuf.payload[0];\n") - payloadSize = 0; - for (i = 0; i < fieldCount; i++) { - if (counts[i]) { - printf("\t_pack(__p, %s, %s);\n", names[i], counts[i]) - payloadSize += sizes[i] * counts[i] - } else { - printf("\t_pack(__p, %s);\n", names[i]) - payloadSize += sizes[i] - } - } - printf("\t_encodeBuf.header.length = %s;\n", payloadSize) - printf("\t_encodeBuf.header.messageID = %s;\n", currentMessage) - printf("\t_encodeBuf.header.messageVersion = MSG_VERSION_1;\n") - printf("\t_sendMessage();\n") - printf("};\n\n") - - # - # emit a routine to unpack the current message into a set of variables - # - printf("/// Unpack a %s message\n", currentMessage) - printf("inline void\nunpack_%s(\n", tolower(currentMessage)) - for (i = 0; i < fieldCount; i++) { - if (counts[i]) { - printf("\t%s (&%s)[%d]", types[i], names[i], counts[i]) - } else { - printf("\t%s &%s", types[i], names[i]) - } - if (i < (fieldCount -1)) - printf(",\n"); - } - printf(")\n{\n") - printf("\tuint8_t *__p = &_decodeBuf.payload[0];\n") - for (i = 0; i < fieldCount; i++) { - if (counts[i]) { - printf("\t_unpack(__p, %s, %s);\n", names[i], counts[i]) - payloadSize += sizes[i] * counts[i] - } else { - printf("\t_unpack(__p, %s);\n", names[i]) - payloadSize += sizes[i] - } - } - printf("};\n") - - # close the Doxygen group - printf("//@}\n") - } -} - -# skip lines containing comments -$1=="#" { - next -} - -# -# process a new message declaration -# -$1=="message" { - - # emit any previous message - EMIT_MESSAGE() - - # save the current opcode and message name - currentOpcode = $2 - currentMessage = $3 - opcodes[$2] = $3 - - # set up for the coming fields - fieldCount = 0 - delete types - delete names - delete sizes - delete counts - - next -} - -# -# process a field inside a message definition -# -NF >= 2 { - - # save the field definition - types[fieldCount] = $1 - names[fieldCount] = $2 - - # guess the field size, note that we only support and "char" - sizes[fieldCount] = 1 - if ($1 ~ ".*16.*") - sizes[fieldCount] = 2 - if ($1 ~ ".*32.*") - sizes[fieldCount] = 4 - - # if an array size was supplied, save it - if (NF >= 3) { - counts[fieldCount] = $3 - } - - fieldCount++ -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile deleted file mode 100644 index a484d5a004..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile +++ /dev/null @@ -1,14 +0,0 @@ - -PROG := BinCommTest -SRCS := test.cpp ../APM_BinComm.cpp -OBJS := $(SRCS:.cpp=.o) - -BinCommTest: $(OBJS) - c++ -g -o $@ $(OBJS) - -.cpp.o: - @echo C++ $< -> $@ - c++ -g -c -I. -o $@ $< - -clean: - rm $(PROG) $(OBJS) diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h deleted file mode 100644 index 539db09c5d..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h +++ /dev/null @@ -1,15 +0,0 @@ - -#ifndef WPROGRAM_H -#define WPROGRAM_H - -class Stream { -public: - void write(uint8_t val); - int available(void); - int read(void); -}; - - -extern unsigned int millis(void); - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp deleted file mode 100644 index e15f1827f0..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- - -// test harness for the APM_BinComm bits - -#include -#include -#include -#include -#include -#include - -#include "WProgram.h" - -#include "../APM_BinComm.h" - -static void handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData); - -BinComm::MessageHandler handlers[] = { - {BinComm::MSG_ANY, handler, NULL}, - {BinComm::MSG_NULL, 0, 0} -}; - -Stream port; -BinComm comm(handlers, &port); - -int port_fd; - -unsigned int -millis(void) -{ - return 0; -} - -void -Stream::write(uint8_t val) -{ - ::write(port_fd, &val, 1); -} - -int -Stream::available(void) -{ - return(1); -} - -int -Stream::read(void) -{ - int ret; - uint8_t c; - - switch(::read(port_fd, &c, 1)) { - case 1: - return c; - case 0: - errx(1, "device disappeared"); - - default: - // almost certainly EWOULDBLOCK - return -1; - } -} - -void -handler(void *arg, uint8_t messageId, uint8_t messageVersion, void *messageData) -{ - - if (messageId == BinComm::MSG_HEARTBEAT) { - struct BinComm::msg_heartbeat *m = (struct BinComm::msg_heartbeat *)messageData; - printf("Heartbeat: mode %u time %u voltage %u command %u\n", - m->flightMode, m->timeStamp, m->batteryVoltage, m->commandIndex); - } else - if (messageId == BinComm::MSG_ATTITUDE) { - struct BinComm::msg_attitude *m = (struct BinComm::msg_attitude *)messageData; - printf("Attitude: pitch %d roll %d yaw %d\n", - m->pitch, m->roll, m->yaw); - } else - if (messageId == BinComm::MSG_LOCATION) { - struct BinComm::msg_location *m = (struct BinComm::msg_location *)messageData; - printf("Location: lat %d long %d altitude %d groundspeed %d groundcourse %d time %u\n", - m->latitude, m->longitude, m->altitude, m->groundSpeed, m->groundCourse, m->timeOfWeek); - } else - if (messageId == BinComm::MSG_STATUS_TEXT) { - struct BinComm::msg_status_text *m = (struct BinComm::msg_status_text *)messageData; - printf("Message %d: %-50s\n", m->severity, m->text); - } else { - warnx("received message %d,%d", messageId, messageVersion); - } -} - -int -main(int argc, char *argv[]) -{ - struct termios t; - - if (2 > argc) - errx(1, "BinCommTest "); - if (0 >= (port_fd = open(argv[1], O_RDWR | O_NONBLOCK))) - err(1, "could not open port %s", argv[1]); - if (tcgetattr(port_fd, &t)) - err(1, "tcgetattr"); - cfsetspeed(&t, 115200); - if (tcsetattr(port_fd, TCSANOW, &t)) - err(1, "tcsetattr"); - - // spin listening - for (;;) { - comm.update(); - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp deleted file mode 100644 index 4b83191ef9..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp +++ /dev/null @@ -1,235 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - APM_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library 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. - - Sensor is conected to I2C port - Sensor is initialized in Continuos mode (10Hz) - - Variables: - Heading : Magnetic heading - Heading_X : Magnetic heading X component - Heading_Y : Magnetic heading Y component - Mag_X : Raw X axis magnetometer data - Mag_Y : Raw Y axis magnetometer data - Mag_Z : Raw Z axis magnetometer data - lastUpdate : the time of the last successful reading - - Methods: - Init() : Initialization of I2C and sensor - Read() : Read Sensor data - Calculate(float roll, float pitch) : Calculate tilt adjusted heading - SetOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass - SetOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances - SetDeclination(float radians) : Set heading adjustment between true north and magnetic north - - To do : code optimization - Mount position : UPDATED - Big capacitor pointing backward, connector forward - -*/ -extern "C" { - // AVR LibC Includes - #include - #include "WConstants.h" -} - -#include -#include "APM_Compass.h" - -#define CompassAddress 0x1E -#define ConfigRegA 0x00 -#define ConfigRegB 0x01 -#define MagGain 0x20 -#define PositiveBiasConfig 0x11 -#define NegativeBiasConfig 0x12 -#define NormalOperation 0x10 -#define ModeRegister 0x02 -#define ContinuousConversion 0x00 -#define SingleConversion 0x01 - -// Constructors //////////////////////////////////////////////////////////////// -APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0) -{ - // mag x y z offset initialisation - offset[0] = 0; - offset[1] = 0; - offset[2] = 0; - - // initialise orientation matrix - orientationMatrix = ROTATION_NONE; -} - -// Public Methods ////////////////////////////////////////////////////////////// -bool APM_Compass_Class::Init(void) -{ - unsigned long currentTime = millis(); // record current time - int numAttempts = 0; - int success = 0; - - Wire.begin(); - - delay(10); - - // calibration initialisation - calibration[0] = 1.0; - calibration[1] = 1.0; - calibration[2] = 1.0; - - while( success == 0 && numAttempts < 5 ) - { - // record number of attempts at initialisation - numAttempts++; - - // force positiveBias (compass should return 715 for all channels) - Wire.beginTransmission(CompassAddress); - Wire.send(ConfigRegA); - Wire.send(PositiveBiasConfig); - if (0 != Wire.endTransmission()) - continue; // compass not responding on the bus - delay(50); - - // set gains - Wire.beginTransmission(CompassAddress); - Wire.send(ConfigRegB); - Wire.send(MagGain); - Wire.endTransmission(); - delay(10); - - Wire.beginTransmission(CompassAddress); - Wire.send(ModeRegister); - Wire.send(SingleConversion); - Wire.endTransmission(); - delay(10); - - // read values from the compass - Read(); - delay(10); - - // calibrate - if( abs(Mag_X) > 500 && abs(Mag_X) < 1000 && abs(Mag_Y) > 500 && abs(Mag_Y) < 1000 && abs(Mag_Z) > 500 && abs(Mag_Z) < 1000) - { - calibration[0] = abs(715.0 / Mag_X); - calibration[1] = abs(715.0 / Mag_Y); - calibration[2] = abs(715.0 / Mag_Z); - - // mark success - success = 1; - } - - // leave test mode - Wire.beginTransmission(CompassAddress); - Wire.send(ConfigRegA); - Wire.send(NormalOperation); - Wire.endTransmission(); - delay(50); - - Wire.beginTransmission(CompassAddress); - Wire.send(ModeRegister); - Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) - Wire.endTransmission(); // End transmission - delay(50); - } - return(success); -} - -// Read Sensor data -void APM_Compass_Class::Read() -{ - int i = 0; - byte buff[6]; - - Wire.beginTransmission(CompassAddress); - Wire.send(0x03); //sends address to read from - Wire.endTransmission(); //end transmission - - //Wire.beginTransmission(CompassAddress); - Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device - while(Wire.available()) - { - buff[i] = Wire.receive(); // receive one byte - i++; - } - Wire.endTransmission(); //end transmission - - if (i==6) // All bytes received? - { - // MSB byte first, then LSB, X,Y,Z - Mag_X = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis - Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis - Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis - lastUpdate = millis(); // record time of update - } -} - -void APM_Compass_Class::Calculate(float roll, float pitch) -{ - float Head_X; - float Head_Y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; - Vector3f rotMagVec; - - cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); - - // rotate the magnetometer values depending upon orientation - if( orientation == 0 ) - rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); - else - rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); - - // Tilt compensated Magnetic field X component: - Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; - // Tilt compensated Magnetic field Y component: - Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll; - // Magnetic Heading - Heading = atan2(-Head_Y,Head_X); - - // Declination correction (if supplied) - if( declination != 0.0 ) - { - Heading = Heading + declination; - if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) - Heading -= (2.0 * M_PI); - else if (Heading < -M_PI) - Heading += (2.0 * M_PI); - } - - // Optimization for external DCM use. Calculate normalized components - Heading_X = cos(Heading); - Heading_Y = sin(Heading); -} - -void APM_Compass_Class::SetOrientation(const Matrix3f &rotationMatrix) -{ - orientationMatrix = rotationMatrix; - if( orientationMatrix == ROTATION_NONE ) - orientation = 0; - else - orientation = 1; -} - -void APM_Compass_Class::SetOffsets(int x, int y, int z) -{ - offset[0] = x; - offset[1] = y; - offset[2] = z; -} - -void APM_Compass_Class::SetDeclination(float radians) -{ - declination = radians; -} - -// make one instance for the user to use -APM_Compass_Class APM_Compass; diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h deleted file mode 100644 index a8d0282a85..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef APM_Compass_h -#define APM_Compass_h - -#include "../AP_Math/AP_Math.h" - -// Rotation matrices -#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) -#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1) -#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1) -#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1) -#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1) -#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1) -#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1) -#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1) -#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1) - -#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE -#define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45 -#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90 -#define APM_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135 -#define APM_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180 -#define APM_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225 -#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270 -#define APM_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180 -#define APM_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90 -#define APM_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180 -#define APM_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225 -#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270 -#define APM_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315 - -class APM_Compass_Class -{ - private: - int orientation; - Matrix3f orientationMatrix; - float calibration[3]; - int offset[3]; - float declination; - public: - int Mag_X; - int Mag_Y; - int Mag_Z; - float Heading; - float Heading_X; - float Heading_Y; - unsigned long lastUpdate; - - APM_Compass_Class(); // Constructor - bool Init(); - void Read(); - void Calculate(float roll, float pitch); - void SetOrientation(const Matrix3f &rotationMatrix); - void SetOffsets(int x, int y, int z); - void SetDeclination(float radians); -}; - -extern APM_Compass_Class APM_Compass; - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde deleted file mode 100644 index 3216f89f55..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde +++ /dev/null @@ -1,87 +0,0 @@ -/* - Example of APM_Compass library (HMC5843 sensor). - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com - - offsets displayed are calculated from the min/max values from each axis. - rotate the compass so as to produce the maximum and minimum values - after the offsets stop changing, edit the code to pass these offsets into - APM_Compass.SetOffsets. -*/ - -#include -#include // Compass Library -#include // Math library - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi - -unsigned long timer; - -void setup() -{ - Serial.begin(38400); - Serial.println("Compass library test (HMC5843)"); - - APM_Compass.Init(); // Initialization - APM_Compass.SetOrientation(APM_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft - APM_Compass.SetOffsets(0,0,0); // set offsets to account for surrounding interference - APM_Compass.SetDeclination(ToRad(0.0)); // set local difference between magnetic north and true north - - delay(1000); - timer = millis(); -} - -void loop() -{ - static float min[3], max[3], offset[3]; - - if((millis()- timer) > 100) - { - timer = millis(); - APM_Compass.Read(); - APM_Compass.Calculate(0,0); // roll = 0, pitch = 0 for this example - - // capture min - if( APM_Compass.Mag_X < min[0] ) - min[0] = APM_Compass.Mag_X; - if( APM_Compass.Mag_Y < min[1] ) - min[1] = APM_Compass.Mag_Y; - if( APM_Compass.Mag_Z < min[2] ) - min[2] = APM_Compass.Mag_Z; - - // capture max - if( APM_Compass.Mag_X > max[0] ) - max[0] = APM_Compass.Mag_X; - if( APM_Compass.Mag_Y > max[1] ) - max[1] = APM_Compass.Mag_Y; - if( APM_Compass.Mag_Z > max[2] ) - max[2] = APM_Compass.Mag_Z; - - // calculate offsets - offset[0] = -(max[0]+min[0])/2; - offset[1] = -(max[1]+min[1])/2; - offset[2] = -(max[2]+min[2])/2; - - // display all to user - Serial.print("Heading:"); - Serial.print(ToDeg(APM_Compass.Heading)); - Serial.print(" ("); - Serial.print(APM_Compass.Mag_X); - Serial.print(","); - Serial.print(APM_Compass.Mag_Y); - Serial.print(","); - Serial.print(APM_Compass.Mag_Z); - Serial.print(")"); - - // display offsets - Serial.print("\t offsets("); - Serial.print(offset[0]); - Serial.print(","); - Serial.print(offset[1]); - Serial.print(","); - Serial.print(offset[2]); - Serial.print(")"); - - Serial.println(); - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt deleted file mode 100644 index 617c9aa805..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt +++ /dev/null @@ -1,19 +0,0 @@ -Compass KEYWORD1 -AP_Compass KEYWORD1 -APM_Compass KEYWORD1 -Init KEYWORD2 -Read KEYWORD2 -Calculate KEYWORD2 -SetOrientation KEYWORD2 -SetOffsets KEYWORDS2 -SetDeclination KEYWORDS2 -Heading KEYWORD2 -Heading_X KEYWORD2 -Heading_Y KEYWORD2 -Mag_X KEYWORD2 -Mag_Y KEYWORD2 -Mag_Z KEYWORD2 -lastUpdate KEYWORD2 - - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp deleted file mode 100644 index 6b811caa5b..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/* - APM_FastSerial.cpp - Fast Serial Output for Ardupilot Mega Hardware (atmega1280) - It´s also compatible with standard Arduino boards (atmega 168 and 328) - Interrupt driven Serial output with intermediate buffer - Code Jose Julio and Jordi Muñoz. DIYDrones.com - - This library 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. - - This library works as a complement of the standard Arduino Serial - library. So user must initialize Standard Serial Arduino library first. - This library works in Serial port 0 and Serial port3(telemetry port)[APM] - Methods: (the same as standard arduino library, inherits from Print) - write() for bytes or array of bytes (binary output) - print() for chars, strings, numbers and floats - println() -*/ - -//#include "WProgram.h" -#include "APM_FastSerial.h" -extern "C" { - // AVR LibC Includes - #include - #include - #include - #include "WConstants.h" -} -#define TX_BUFFER_SIZE 80 // Serial output buffer size - -// Serial0 buffer -uint8_t tx_buffer0[TX_BUFFER_SIZE]; -volatile int tx_buffer0_head=0; -volatile int tx_buffer0_tail=0; - -#if defined(__AVR_ATmega1280__) -// Serial3 buffer -uint8_t tx_buffer3[TX_BUFFER_SIZE]; -volatile int tx_buffer3_head=0; -volatile int tx_buffer3_tail=0; -#endif - -#if defined(__AVR_ATmega1280__) // For atmega1280 we use Serial port 0 and 3 -// Serial0 interrupt -ISR(SIG_USART0_DATA) -{ - uint8_t data; - - if (tx_buffer0_tail == tx_buffer0_head) - UCSR0B &= ~(_BV(UDRIE0)); // Disable interrupt - else { - data = tx_buffer0[tx_buffer0_tail]; - tx_buffer0_tail = (tx_buffer0_tail + 1) % TX_BUFFER_SIZE; - UDR0 = data; - } -} - -// Serial3 interrupt -ISR(SIG_USART3_DATA) -{ - uint8_t data; - - if (tx_buffer3_tail == tx_buffer3_head) - UCSR3B &= ~(_BV(UDRIE3)); // Disable interrupt - else { - data = tx_buffer3[tx_buffer3_tail]; - tx_buffer3_tail = (tx_buffer3_tail + 1) % TX_BUFFER_SIZE; - UDR3 = data; - } -} -#else - -// Serial interrupt -ISR(USART_UDRE_vect) -{ - uint8_t data; - - if (tx_buffer0_tail == tx_buffer0_head) - UCSR0B &= ~(_BV(UDRIE0)); // Disable interrupt - else { - data = tx_buffer0[tx_buffer0_tail]; - tx_buffer0_tail = (tx_buffer0_tail + 1) % TX_BUFFER_SIZE; - UDR0 = data; - } -} -#endif - -// Constructors //////////////////////////////////////////////////////////////// -APM_FastSerial_Class::APM_FastSerial_Class(uint8_t SerialPort) -{ - SerialPortNumber=SerialPort; // This could be serial port 0 or 3 -} - -// Public Methods ////////////////////////////////////////////////////////////// - -// This is the important function (basic funtion: send a byte) -void APM_FastSerial_Class::write(uint8_t b) -{ - uint8_t Enable_tx_int=0; - uint8_t new_head; - - if (SerialPortNumber==0) // Serial Port 0 - { - // if buffer was empty then we enable Serial TX interrupt - if (tx_buffer0_tail == tx_buffer0_head) - Enable_tx_int=1; - - new_head = (tx_buffer0_head + 1) % TX_BUFFER_SIZE; // Move to next position in the ring buffer - if (new_head==tx_buffer0_tail) - return; // This is an Error : BUFFER OVERFLOW. We lost this character!! - - tx_buffer0[tx_buffer0_head] = b; // Add data to the tx buffer - tx_buffer0_head = new_head; // Update head pointer - - if (Enable_tx_int) - UCSR0B |= _BV(UDRIE0); // Enable Serial TX interrupt - } -#if defined(__AVR_ATmega1280__) - else // Serial Port 3 - { - // if buffer was empty then we enable Serial TX interrupt - if (tx_buffer3_tail == tx_buffer3_head) - Enable_tx_int=1; - - new_head = (tx_buffer3_head + 1) % TX_BUFFER_SIZE; // Move to next position in the ring buffer - if (new_head==tx_buffer3_tail) - return; // This is an Error : BUFFER OVERFLOW. We lost this character!! - - tx_buffer3[tx_buffer3_head] = b; // Add data to the tx buffer - tx_buffer3_head = new_head; // Update head pointer - - if (Enable_tx_int) - UCSR3B |= _BV(UDRIE3); // Enable Serial TX interrupt - } -#endif -} - -// Send a buffer of bytes (this is util for binary protocols) -void APM_FastSerial_Class::write(const uint8_t *buffer, int size) -{ - while (size--) - write(*buffer++); -} - -// We create this two instances -APM_FastSerial_Class APM_FastSerial(0); // For Serial port 0 -#if defined(__AVR_ATmega1280__) - APM_FastSerial_Class APM_FastSerial3(3); // For Serial port 3 (only Atmega1280) -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h deleted file mode 100644 index 319ec523dd..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef APM_FastSerial_h -#define APM_FastSerial_h - -#include - -#include "Print.h" - -class APM_FastSerial_Class : public Print // Inherit from Print -{ - private: - uint8_t SerialPortNumber; - - public: - APM_FastSerial_Class(uint8_t SerialPort); // Constructor - // we overwrite the write methods - void write(uint8_t b); // basic funtion : send a byte - void write(const uint8_t *buffer, int size); -}; - -extern APM_FastSerial_Class APM_FastSerial; -#if defined(__AVR_ATmega1280__) - extern APM_FastSerial_Class APM_FastSerial3; -#endif -#endif - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde deleted file mode 100644 index 97616237ed..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde +++ /dev/null @@ -1,91 +0,0 @@ -/* - Example of APM_FastSerial library. - Code by Jose Julio and Jordi Muñoz. DIYDrones.com -*/ - -// FastSerial is implemented for Serial port 0 (connection to PC) and 3 (telemetry) -#include // ArduPilot Mega Fast Serial Output Library - -#if defined(__AVR_ATmega1280__) - #define LED 35 -#else - #define LED 13 -#endif - -unsigned long timer; -unsigned long counter; - -void setup() -{ - int myint=14235; // Examples of data tytpes - long mylong=-23456432; - float myfloat=-26.669; - byte mybyte=0xD1; - byte bc_bufIn[50]; - - for (int i=0;i<10;i++) - bc_bufIn[i]=i*10+30; // we fill the byte array - - pinMode(LED,OUTPUT); - - // We use the standard serial port initialization - Serial.begin(57600); - //Serial3.begin(57600); // if we want to use port3 also (Mega boards)... - delay(100); - // We can use both methods to write to serial port: - Serial.println("ArduPilot Mega FastSerial library test"); // Standard serial output - APM_FastSerial.println("FAST_SERIAL : ArduPilot Mega FastSerial library test"); // Fast serial output - // We can use the same on serial port3 (telemetry) - // APM_FastSerial3.println("Serial Port3 : ArduPilot Mega FastSerial library test"); - delay(1000); - // Examples of data types (same result as standard arduino library) - APM_FastSerial.println("Numbers:"); - APM_FastSerial.println(myint); - APM_FastSerial.println(mylong); - APM_FastSerial.println(myfloat); - APM_FastSerial.println("Byte:"); - APM_FastSerial.write(mybyte); - APM_FastSerial.println(); - APM_FastSerial.println("Bytes:"); - APM_FastSerial.write(bc_bufIn,10); - APM_FastSerial.println(); - delay(4000); -} - -void loop() -{ - if((millis()- timer) > 20) // 50Hz loop - { - timer = millis(); - if (counter < 250) // we use the Normal Serial output for 5 seconds - { - // Example of typical telemetry output - digitalWrite(LED,HIGH); - Serial.println("!ATT:14.2;-5.5;-26.8"); // 20 bytes - digitalWrite(LED,LOW); - if ((counter%5)==0) // GPS INFO at 5Hz - { - digitalWrite(LED,HIGH); - Serial.println("!LAT:26.345324;LON:-57.372638;ALT:46.7;GC:121.3;GS:23.1"); // 54 bytes - digitalWrite(LED,LOW); - } - } - else // and Fast Serial Output for other 5 seconds - { - // The same info... - digitalWrite(LED,HIGH); - APM_FastSerial.println("#ATT:14.2;-5.5;-26.8"); // 20 bytes - digitalWrite(LED,LOW); - if ((counter%5)==0) // GPS INFO at 5Hz - { - digitalWrite(LED,HIGH); - - APM_FastSerial.println("#LAT:26.345324;LON:-57.372638;ALT:46.7;GC:121.3;GS:23.1"); // 54 bytes - digitalWrite(LED,LOW); - } - if (counter>500) // Counter reset - counter=0; - } - counter++; - } -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt deleted file mode 100644 index 91cfc02dea..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt +++ /dev/null @@ -1,2 +0,0 @@ -APM_FastSerial KEYWORD1 -APM_FastSerial3 KEYWORD1 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp deleted file mode 100644 index 7099b44e26..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* - APM_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library 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. - - RC Input : PPM signal on IC4 pin - RC Output : 11 Servo outputs (standard 20ms frame) - - Methods: - Init() : Initialization of interrupts an Timers - OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10 - InputCh(ch) : Read a channel input value. ch=0..7 - GetState() : Returns the state of the input. 1 => New radio frame to process - Automatically resets when we call InputCh to read channels - -*/ -#include "APM_RC.h" - -#include -#include "WProgram.h" - -#if !defined(__AVR_ATmega1280__) -# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. -#else - -// Variable definition for Input Capture interrupt -volatile unsigned int ICR4_old; -volatile unsigned char PPM_Counter=0; -volatile unsigned int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400}; -volatile unsigned char radio_status=0; - -/**************************************************** - Input Capture Interrupt ICP4 => PPM signal read - ****************************************************/ -ISR(TIMER4_CAPT_vect) -{ - unsigned int Pulse; - unsigned int Pulse_Width; - - Pulse=ICR4; - if (Pulse8000) // SYNC pulse? - PPM_Counter=0; - else - { - PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) - PWM_RAW[PPM_Counter++]=Pulse_Width; //Saving pulse. - if (PPM_Counter >= NUM_CHANNELS) - radio_status = 1; - } - ICR4_old = Pulse; -} - - -// Constructors //////////////////////////////////////////////////////////////// - -APM_RC_Class::APM_RC_Class() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void APM_RC_Class::Init(void) -{ - // Init PWM Timer 1 - pinMode(11,OUTPUT); // (PB5/OC1A) - pinMode(12,OUTPUT); //OUT2 (PB6/OC1B) - pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) - - //Remember the registers not declared here remains zero by default... - TCCR1A =((1<>1; // Because timer runs at 0.5us we need to do value/2 - result2 = PWM_RAW[ch]>>1; - if (result != result2) - result = PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine) - - // Limit values to a valid range - result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); - radio_status=0; // Radio channel read - return(result); -} - -unsigned char APM_RC_Class::GetState(void) -{ - return(radio_status); -} - -// InstantPWM implementation -// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use -void APM_RC_Class::Force_Out0_Out1(void) -{ - if (TCNT5>5000) // We take care that there are not a pulse in the output - TCNT5=39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 -} -// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use -void APM_RC_Class::Force_Out2_Out3(void) -{ - if (TCNT1>5000) - TCNT1=39990; -} -// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use -void APM_RC_Class::Force_Out6_Out7(void) -{ - if (TCNT3>5000) - TCNT3=39990; -} - -// make one instance for the user to use -APM_RC_Class APM_RC; - -#endif // defined(ATMega1280) diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h deleted file mode 100644 index 3874e0a319..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef APM_RC_h -#define APM_RC_h - -#define NUM_CHANNELS 8 -#define MIN_PULSEWIDTH 900 -#define MAX_PULSEWIDTH 2100 - -class APM_RC_Class -{ - private: - public: - APM_RC_Class(); - void Init(); - void OutputCh(unsigned char ch, int pwm); - int InputCh(unsigned char ch); - unsigned char GetState(); - void Force_Out0_Out1(void); - void Force_Out2_Out3(void); - void Force_Out6_Out7(void); -}; - -extern APM_RC_Class APM_RC; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde deleted file mode 100644 index 3edc3d223a..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde +++ /dev/null @@ -1,31 +0,0 @@ -/* - Example of APM_RC library. - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - Print Input values and send Output to the servos - (Works with last PPM_encoder firmware) -*/ - -#include // ArduPilot Mega RC Library - -void setup() -{ - APM_RC.Init(); // APM Radio initialization - Serial.begin(57600); - Serial.println("ArduPilot Mega RC library test"); - delay(1000); -} -void loop() -{ - if (APM_RC.GetState()==1) // New radio frame? (we could use also if((millis()- timer) > 20) - { - 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(); - } -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt deleted file mode 100644 index 0fba1dd8a0..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt +++ /dev/null @@ -1,8 +0,0 @@ -APM_RC KEYWORD1 -begin KEYWORD2 -InputCh KEYWORD2 -OutputCh KEYWORD2 -GetState KEYWORD2 -Force_Out0_Out1 KEYWORD2 -Force_Out2_Out3 KEYWORD2 -Force_Out6_Out7 KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp deleted file mode 100644 index da5f4dd4a9..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/* - APM_RC_QUAD.cpp - Radio Control Library for Ardupilot Mega. Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library 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. - - This is a special version of the library for use in Quadcopters - Because we have modified servo outputs 0..3 to have a higher rate (300Hz) - - RC Input : PPM signal on IC4 pin - RC Output : 8 servo Outputs, - OUT0..OUT3 : 300Hz Servo output (for Quad ESC´s) - OUT4..OUT7 : standard 50Hz servo outputs - - Methods: - Init() : Initialization of interrupts an Timers - OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10 - InputCh(ch) : Read a channel input value. ch=0..7 - GetState() : Returns the state of the input. 1 => New radio frame to process - Automatically resets when we call InputCh to read channels - -*/ -#include "APM_RC_QUAD.h" - -#include -#include "WProgram.h" - -// Variable definition for Input Capture interrupt -volatile unsigned int ICR4_old; -volatile unsigned char PPM_Counter=0; -volatile unsigned int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400}; -volatile unsigned char radio_status=0; - -/**************************************************** - Input Capture Interrupt ICP4 => PPM signal read - ****************************************************/ -ISR(TIMER4_CAPT_vect) -{ - unsigned int Pulse; - unsigned int Pulse_Width; - - Pulse=ICR4; - if (Pulse8000) // SYNC pulse? - PPM_Counter=0; - else - { - PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) - PWM_RAW[PPM_Counter++]=Pulse_Width; //Saving pulse. - if (PPM_Counter >= NUM_CHANNELS) - radio_status = 1; - } - ICR4_old = Pulse; -} - - -// Constructors //////////////////////////////////////////////////////////////// - -APM_RC_QUAD_Class::APM_RC_QUAD_Class() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void APM_RC_QUAD_Class::Init(void) -{ - // Init PWM Timer 1 - //pinMode(11,OUTPUT); // (PB5/OC1A) - pinMode(12,OUTPUT); //OUT2 (PB6/OC1B) - pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) - - //Remember the registers not declared here remains zero by default... - TCCR1A =((1<>1; // Because timer runs at 0.5us we need to do value/2 - result2 = PWM_RAW[ch]>>1; - if (result != result2) - result = PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine) - - // Limit values to a valid range - result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); - radio_status=0; // Radio channel read - return(result); -} - -unsigned char APM_RC_QUAD_Class::GetState(void) -{ - return(radio_status); -} - -// make one instance for the user to use -APM_RC_QUAD_Class APM_RC_QUAD; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h deleted file mode 100644 index 421089d7af..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef APM_RC_QUAD_h -#define APM_RC_QUAD_h - -#define NUM_CHANNELS 8 -#define MIN_PULSEWIDTH 900 -#define MAX_PULSEWIDTH 2100 - -class APM_RC_QUAD_Class -{ - private: - public: - APM_RC_QUAD_Class(); - void Init(); - void OutputCh(unsigned char ch, int pwm); - int InputCh(unsigned char ch); - unsigned char GetState(); -}; - -extern APM_RC_QUAD_Class APM_RC_QUAD; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde deleted file mode 100644 index 3edc3d223a..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde +++ /dev/null @@ -1,31 +0,0 @@ -/* - Example of APM_RC library. - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - Print Input values and send Output to the servos - (Works with last PPM_encoder firmware) -*/ - -#include // ArduPilot Mega RC Library - -void setup() -{ - APM_RC.Init(); // APM Radio initialization - Serial.begin(57600); - Serial.println("ArduPilot Mega RC library test"); - delay(1000); -} -void loop() -{ - if (APM_RC.GetState()==1) // New radio frame? (we could use also if((millis()- timer) > 20) - { - 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(); - } -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt deleted file mode 100644 index 762966e545..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt +++ /dev/null @@ -1,5 +0,0 @@ -APM_RC_QUAD KEYWORD1 -begin KEYWORD2 -InputCh KEYWORD2 -OutputCh KEYWORD2 -GetState KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp deleted file mode 100644 index 5b476c4f09..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// This 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. -// - -/// @file AP_Common.cpp -/// @brief Common utility routines for the ArduPilot libraries. -/// -/// @note Exercise restraint adding code here; everything in this -/// library will be linked with any sketch using it. -/// - -#include "AP_Common.h" - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h deleted file mode 100644 index 8ad2f0fbf5..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h +++ /dev/null @@ -1,53 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// This 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. -// - -/// -/// @file AP_Common.h -/// @brief Common definitions and utility routines for the ArduPilot -/// libraries. -/// - -#ifndef _AP_COMMON_H -#define _AP_COMMON_H - -#include - -#include "include/menu.h" /// simple menu subsystem - -//////////////////////////////////////////////////////////////////////////////// -/// @name Types -/// -/// Data structures and types used throughout the libraries and applications. -/// -//@{ - -struct Location { - uint8_t id; ///< command id - uint8_t p1; ///< param 1 - int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100) - int32_t lat; ///< param 3 - Lattitude * 10**7 - int32_t lng; ///< param 4 - Longitude * 10**7 -}; - -//@} - -//////////////////////////////////////////////////////////////////////////////// -/// @name Conversions -/// -/// Conversion macros and factors. -/// -//@{ - -/// XXX this should probably be replaced with radians()/degrees(), but their -/// inclusion in wiring.h makes doing that here difficult. -#define ToDeg(x) (x*57.2957795131) // *180/pi - -//@} - - -#endif // _AP_COMMON_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp deleted file mode 100644 index 107d7fb045..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp +++ /dev/null @@ -1,21 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// -// C++ runtime support not provided by Arduino -// -// Note: use new/delete with caution. The heap is small and -// easily fragmented. -// - -#include - -void * operator new(size_t size) -{ - return(calloc(size, 1)); -} - -void operator delete(void *p) -{ - if (p) - free(p); -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde deleted file mode 100644 index fea9efa188..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde +++ /dev/null @@ -1,45 +0,0 @@ - -#include -#include - -#include - -FastSerialPort0(Serial); - -int -menu_test(uint8_t argc, const Menu::arg *argv) -{ - int i; - - Serial.printf("This is a test with %d arguments\n", argc); - for (i = 1; i < argc; i++) { - Serial.printf("%d: int %ld float ", i, argv[i].i); - Serial.println(argv[i].f, 6); // gross - } -} - -int -menu_auto(uint8_t argc, const Menu::arg *argv) -{ - Serial.println("auto text"); -} - -const struct Menu::command top_menu_commands[] PROGMEM = { - {"*", menu_auto}, - {"test", menu_test}, -}; - -MENU(top, "menu", top_menu_commands); - -void -setup(void) -{ - Serial.begin(38400); - top.run(); -} - -void -loop(void) -{ -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h deleted file mode 100644 index 96b863c05b..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h +++ /dev/null @@ -1,138 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file menu.h -/// @brief Simple commandline menu subsystem. -/// @discussion -/// The Menu class implements a simple CLI that accepts commands typed by -/// the user, and passes the arguments to those commands to a function -/// defined as handing the command. -/// -/// Commands are defined in an array of Menu::command structures passed -/// to the constructor. Each entry in the array defines one command. -/// -/// Arguments passed to the handler function are pre-converted to both -/// long and float for convenience. -/// - -#define MENU_COMMANDLINE_MAX 32 ///< maximum input line length -#define MENU_ARGS_MAX 4 ///< maximum number of arguments -#define MENU_COMMAND_MAX 14 ///< maximum size of a command name - -/// Class defining and handling one menu tree -class Menu { -public: - /// argument passed to a menu function - /// - /// Space-delimited arguments are parsed from the commandline and - /// separated into these structures. - /// - /// If the argument cannot be parsed as a float or a long, the value - /// of f or i respectively is undefined. You should range-check - /// the inputs to your function. - /// - struct arg { - const char *str; ///< string form of the argument - long i; ///< integer form of the argument (if a number) - float f; ///< floating point form of the argument (if a number) - }; - - /// menu command function - /// - /// Functions called by menu array entries are expected to be of this - /// type. - /// - /// @param argc The number of valid arguments, including the - /// name of the command in argv[0]. Will never be - /// more than MENU_ARGS_MAX. - /// @param argv Pointer to an array of Menu::arg structures - /// detailing any optional arguments given to the - /// command. argv[0] is always the name of the - /// command, so that the same function can be used - /// to handle more than one command. - /// - typedef int8_t (*func)(uint8_t argc, const struct arg *argv); - - /// menu pre-prompt function - /// - /// Called immediately before waiting for the user to type a command; can be - /// used to display help text or status, for example. - /// - /// If this function returns false, the menu exits. - /// - typedef bool (*preprompt)(void); - - /// menu command description - /// - struct command { - /// Name of the command, as typed or received. - /// Command names are limited in size to keep this structure compact. - /// - const char command[MENU_COMMAND_MAX]; - - /// The function to call when the command is received. - /// - /// The argc argument will be at least 1, and no more than - /// MENU_ARGS_MAX. The argv array will be populated with - /// arguments typed/received up to MENU_ARGS_MAX. The command - /// name will always be in argv[0]. - /// - /// Commands may return -2 to cause the menu itself to exit. - /// The "?", "help" and "exit" commands are always defined, but - /// can be overridden by explicit entries in the command array. - /// - int8_t (*func)(uint8_t argc, const struct arg *argv); ///< callback function - }; - - /// constructor - /// - /// Note that you should normally not call the constructor directly. Use - /// the MENU and MENU2 macros defined below. - /// - /// @param prompt The prompt to be displayed with this menu. - /// @param commands An array of ::command structures in program memory (PROGMEM). - /// @param entries The number of entries in the menu. - /// - Menu(const char *prompt, const struct command *commands, uint8_t entries, preprompt ppfunc = 0); - - /// menu runner - void run(void); - -private: - /// Implements the default 'help' command. - /// - void _help(void); ///< implements the 'help' command - - /// calls the function for the n'th menu item - /// - /// @param n Index for the menu item to call - /// @param argc Number of arguments prepared for the menu item - /// - int8_t _call(uint8_t n, uint8_t argc); - - const char *_prompt; ///< prompt to display - const command *_commands; ///< array of commands - const uint8_t _entries; ///< size of the menu - const preprompt _ppfunc; ///< optional pre-prompt action - - static char _inbuf[MENU_COMMANDLINE_MAX]; ///< input buffer - static arg _argv[MENU_ARGS_MAX + 1]; ///< arguments -}; - -/// Macros used to define a menu. -/// -/// The commands argument should be an arary of Menu::command structures, one -/// per command name. The array does not need to be terminated with any special -/// record. -/// -/// Use name.run() to run the menu. -/// -/// The MENU2 macro supports the optional pre-prompt printing function. -/// -#define MENU(name, prompt, commands) \ - static const char __menu_name__ ##name[] PROGMEM = prompt; \ - static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0])) - -#define MENU2(name, prompt, commands, preprompt) \ - static const char __menu_name__ ##name[] PROGMEM = prompt; \ - static Menu name(__menu_name__ ##name, commands, sizeof(commands) / sizeof(commands[0]), preprompt) - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt deleted file mode 100644 index 470e71f7a8..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt +++ /dev/null @@ -1,4 +0,0 @@ -Menu KEYWORD1 -run KEYWORD2 -Location KEYWORD2 - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp deleted file mode 100644 index 58e129d03e..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp +++ /dev/null @@ -1,129 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// -// Simple commandline menu system. -// - -#include - -#include -#include -#include - -#include "include/menu.h" - -// statics -char Menu::_inbuf[MENU_COMMANDLINE_MAX]; -Menu::arg Menu::_argv[MENU_ARGS_MAX + 1]; - -// constructor -Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entries, preprompt ppfunc) : - _prompt(prompt), - _commands(commands), - _entries(entries), - _ppfunc(ppfunc) -{ -} - -// run the menu -void -Menu::run(void) -{ - uint8_t len, i, ret; - uint8_t argc; - int c; - char *s; - - // loop performing commands - for (;;) { - - // run the pre-prompt function, if one is defined - if ((NULL != _ppfunc) && !_ppfunc()) - return; - - // loop reading characters from the input - len = 0; - Serial.printf("%S] ", _prompt); - for (;;) { - c = Serial.read(); - if (-1 == c) - continue; - // carriage return -> process command - if ('\r' == c) { - _inbuf[len] = '\0'; - Serial.write('\r'); - Serial.write('\n'); - break; - } - // backspace - if ('\b' == c) { - if (len > 0) { - len--; - Serial.write('\b'); - Serial.write(' '); - Serial.write('\b'); - continue; - } - } - // printable character - if (isprint(c) && (len < (MENU_COMMANDLINE_MAX - 1))) { - _inbuf[len++] = c; - Serial.write((char)c); - continue; - } - } - - // split the input line into tokens - argc = 0; - _argv[argc++].str = strtok_r(_inbuf, " ", &s); - // XXX should an empty line by itself back out of the current menu? - while (argc <= MENU_ARGS_MAX) { - _argv[argc].str = strtok_r(NULL, " ", &s); - if ('\0' == _argv[argc].str) - break; - _argv[argc].i = atol(_argv[argc].str); - _argv[argc].f = atof(_argv[argc].str); // calls strtod, > 700B ! - argc++; - } - - // look for a command matching the first word (note that it may be empty) - for (i = 0; i < _entries; i++) { - if (!strcasecmp_P(_argv[0].str, _commands[i].command)) { - ret = _call(i, argc); - if (-2 == ret) - return; - break; - } - } - - // implicit commands - if (i == _entries) { - if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) { - _help(); - } else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) { - return; - } - } - } -} - -// display the list of commands in response to the 'help' command -void -Menu::_help(void) -{ - int i; - - Serial.println("Commands:"); - for (i = 0; i < _entries; i++) - Serial.printf(" %S\n", _commands[i].command); -} - -// run the n'th command in the menu -int8_t -Menu::_call(uint8_t n, uint8_t argc) -{ - func fn; - - fn = (func)pgm_read_word(&_commands[n].func); - return(fn(argc, &_argv[0])); -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp deleted file mode 100644 index a92cfa7acc..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* - AP_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library 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. - - Sensor is conected to I2C port - Sensor is initialized in Continuos mode (10Hz) - - Variables: - heading : Magnetic heading - heading_X : Magnetic heading X component - heading_Y : Magnetic heading Y component - mag_X : Raw X axis magnetometer data - mag_Y : Raw Y axis magnetometer data - mag_Z : Raw Z axis magnetometer data - - Methods: - init() : initialization of I2C and sensor - update() : update Sensor data - - To do : Calibration of the sensor, code optimization - Mount position : UPDATED - Big capacitor pointing backward, connector forward - -*/ - - -extern "C" { - // AVR LibC Includes - #include - #include "WConstants.h" -} - -#include -#include "AP_Compass.h" - -#define COMPASS_ADDRESS 0x1E - -// Constructors //////////////////////////////////////////////////////////////// -AP_Compass::AP_Compass() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void -AP_Compass::init(void) -{ - Wire.begin(); - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(0x02); - Wire.send(0x00); // Set continouos mode (default to 10Hz) - Wire.endTransmission(); // end transmission -} - -// update Sensor data -void -AP_Compass::update() -{ - int i = 0; - byte buff[6]; - - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(0x03); // sends address to read from - Wire.endTransmission(); // end transmission - - //Wire.beginTransmission(COMPASS_ADDRESS); - Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device - while(Wire.available()){ - buff[i] = Wire.receive(); // receive one byte - i++; - } - Wire.endTransmission(); // end transmission - - // All bytes received? - if (i == 6) { - // MSB byte first, then LSB, X,Y,Z - mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis - mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis - mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis - } -} - -void -AP_Compass::calculate(float roll, float pitch) -{ - float head_X; - float head_Y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; - - cos_roll = cos(roll); // Optimization, you can get this out of the matrix DCM? - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); - - // Tilt compensated Magnetic field X component: - head_X = mag_X * cos_pitch + mag_Y * sin_roll * sin_pitch + mag_Z * cos_roll * sin_pitch; - - // Tilt compensated Magnetic field Y component: - head_Y = mag_Y * cos_roll - mag_Z * sin_roll; - - // Magnetic heading - heading = atan2(-head_Y, head_X); - ground_course = (degrees(heading) + 180) * 100; - - // Optimization for external DCM use. calculate normalized components - heading_X = cos(heading); - heading_Y = sin(heading); -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h deleted file mode 100644 index cbafb19e18..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef AP_Compass_h -#define AP_Compass_h - -#include - -class AP_Compass : public Compass -{ - public: - AP_Compass(); // Constructor - void init(); - void update(); - void calculate(float roll, float pitch); - - private: -}; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h deleted file mode 100644 index 0852cdd52a..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef Compass_h -#define Compass_h - -#include - -class Compass -{ - public: - virtual void init(); - virtual void update(); - virtual void calculate(float roll, float pitch); - - int16_t mag_X; - int16_t mag_Y; - int16_t mag_Z; - int32_t ground_course; - float heading; - float heading_X; - float heading_Y; - -}; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde deleted file mode 100644 index 1a69883789..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ /dev/null @@ -1,40 +0,0 @@ -/* - Example of APM_Compass library (HMC5843 sensor). - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com -*/ - -#include -#include // Compass Library - -AP_Compass compass; - -unsigned long timer; - -void setup() -{ - compass.init(); // Initialization - Serial.begin(38400); - Serial.println("AP Compass library test (HMC5843)"); - delay(1000); - timer = millis(); -} - -void loop() -{ - float tmp; - - if((millis()- timer) > 100){ - timer = millis(); - compass.update(); - compass.calculate(0, 0); // roll = 0, pitch = 0 for this example - Serial.print("Heading:"); - Serial.print(compass.ground_course,DEC); - Serial.print(" ("); - Serial.print(compass.mag_X); - Serial.print(","); - Serial.print(compass.mag_Y); - Serial.print(","); - Serial.print(compass.mag_Z); - Serial.println(" )"); - } -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt deleted file mode 100644 index 91affe3e73..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt +++ /dev/null @@ -1,15 +0,0 @@ -Compass KEYWORD1 -AP_Compass KEYWORD1 -APM_Compass KEYWORD1 -init KEYWORD2 -update KEYWORD2 -calculate KEYWORD2 -heading KEYWORD2 -heading_X KEYWORD2 -heading_Y KEYWORD2 -mag_X KEYWORD2 -mag_Y KEYWORD2 -mag_Z KEYWORD2 - - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h deleted file mode 100644 index 384d7daccc..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h +++ /dev/null @@ -1,13 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file AP_GPS.h -/// @brief Catch-all header that defines all supported GPS classes. - -#include "AP_GPS_NMEA.h" -#include "AP_GPS_SIRF.h" -#include "AP_GPS_406.h" -#include "AP_GPS_UBLOX.h" -#include "AP_GPS_MTK.h" -#include "AP_GPS_IMU.h" -#include "AP_GPS_None.h" -#include "AP_GPS_Auto.h" diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp deleted file mode 100644 index 379d096cdd..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// GPS_406.cpp - 406 GPS library for Arduino -// Code by Michael Smith, Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com -// This code works with boards based on ATMega168/328 ATMega1280 (Serial port 1) -// -// This library 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. - -#include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh. -#include "AP_GPS_406.h" -#include "WProgram.h" - -static const char init_str[] = "$PSRF100,0,57600,8,1,0*37"; - -// Constructors //////////////////////////////////////////////////////////////// -AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s) -{ -} - -// Public Methods //////////////////////////////////////////////////////////////////// -void AP_GPS_406::init(void) -{ - _change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate - _configure_gps(); // Function to configure GPS, to output only the desired msg's - - AP_GPS_SIRF::init(); // let the superclass do anything it might need here -} - -// Private Methods ////////////////////////////////////////////////////////////// - -void -AP_GPS_406::_configure_gps(void) -{ - const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00}; - const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B}; - const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1}; - const uint8_t gps_ender[] = {0xB0, 0xB3}; - - for(int z = 0; z < 2; z++){ - for(int x = 0; x < 5; x++){ - _port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg.. - _port->write(gps_payload[x]); // Prints the payload, is not the same for every msg - for(int y = 0; y < 6; y++) // Prints 6 zeros - _port->write((uint8_t)0); - _port->write(gps_checksum[x]); // Print the Checksum - _port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's. - _port->write(gps_ender[1]); // ender - } - } -} - -// The EM406 defalts to NMEA at 4800bps. We want to switch it to SiRF binary -// mode at a higher rate. -// -// The change is sticky, but only for as long as the internal supercap holds -// settings (usually less than a week). -// -void -AP_GPS_406::_change_to_sirf_protocol(void) -{ - FastSerial *fs = (FastSerial *)_port; // this is a bit grody... - - fs->begin(4800); - delay(300); - _port->print(init_str); - delay(300); - - fs->begin(9600); - delay(300); - _port->print(init_str); - delay(300); - - fs->begin(GPS_406_BITRATE); - delay(300); - _port->print(init_str); - delay(300); -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h deleted file mode 100644 index 39b6e55164..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h +++ /dev/null @@ -1,32 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// EM406 GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith, Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com -// -// This library 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. -// - -#ifndef AP_GPS_406_h -#define AP_GPS_406_h - -#include "AP_GPS_SIRF.h" - -#define GPS_406_BITRATE 57600 - -class AP_GPS_406 : public AP_GPS_SIRF -{ -public: - // Methods - AP_GPS_406(Stream *port); - void init(void); - -private: - void _change_to_sirf_protocol(void); - void _configure_gps(void); -}; - -#endif - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp deleted file mode 100644 index 727b1eac74..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp +++ /dev/null @@ -1,162 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// Auto-detecting pseudo-GPS driver -// - -#include "AP_GPS.h" -#include -#include -#include - -static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U}; - -void -AP_GPS_Auto::init(void) -{ -} - -// -// Called the first time that a client tries to kick the GPS to update. -// -// We detect the real GPS, then update the pointer we have been called through -// and return. -void -AP_GPS_Auto::update(void) -{ - GPS *gps; - int i; - - // loop trying to find a GPS - for (;;) { - // loop through possible baudrates - for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { - printf("GPS autodetect at %d:%u\n", i, baudrates[i]); - _port->begin(baudrates[i]); - if (NULL != (gps = _detect())) { - // make the detected GPS the default - *_gps = gps; - - // configure the detected GPS and run one update - gps->print_errors = true; // XXX - gps->init(); - gps->update(); - - // drop back to our caller - subsequent calls through - // the global will not come here - return; - } - } - } -} - -// -// Perform one iteration of the auto-detection process. -// -GPS * -AP_GPS_Auto::_detect(void) -{ - unsigned long then; - int fingerprint[4]; - int tries; - GPS *gps; - - // - // Loop attempting to detect a recognised GPS - // - gps = NULL; - for (tries = 0; tries < 2; tries++) { - - // - // Empty the serial buffer and wait for 50ms of quiet. - // - // XXX We can detect babble by counting incoming characters, but - // what would we do about it? - // - _port->flush(); - then = millis(); - do { - if (_port->available()) { - then = millis(); - _port->read(); - } - } while ((millis() - then) < 50); - - // - // Collect four characters to fingerprint a device - // - fingerprint[0] = _getc(); - fingerprint[1] = _getc(); - fingerprint[2] = _getc(); - fingerprint[3] = _getc(); - printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n", - fingerprint[0], - fingerprint[1], - fingerprint[2], - fingerprint[3]); - - // - // u-blox or MTK in DIYD binary mode (whose smart idea was - // it to make the MTK look sort-of like it was talking UBX?) - // - if ((0xb5 == fingerprint[0]) && - (0x62 == fingerprint[1]) && - (0x01 == fingerprint[2])) { - - // message 5 is MTK pretending to talk UBX - if (0x05 == fingerprint[3]) { - printf("detected MTK in binary mode\n"); - gps = new AP_GPS_MTK(_port); - break; - } - - // any other message is u-blox - printf("detected u-blox in binary mode\n"); - gps = new AP_GPS_UBLOX(_port); - break; - } - - // - // SIRF in binary mode - // - if ((0xa0 == fingerprint[0]) && - (0xa2 == fingerprint[1])) { - printf("detected SIRF in binary mode\n"); - gps = new AP_GPS_SIRF(_port); - break; - } - - // - // If we haven't spammed the various init strings, send them now - // and retry to avoid a false-positive on the NMEA detector. - // - if (0 == tries) { - printf("sending setup strings and trying again\n"); - _port->println(MTK_SET_BINARY); - _port->println(UBLOX_SET_BINARY); - _port->println(SIRF_SET_BINARY); - continue; - } - - // - // Something talking NMEA - // - if (('$' == fingerprint[0]) && - (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) { - - // XXX this may be a bit presumptive, might want to give the GPS a couple of - // iterations around the loop to react to init strings? - printf("detected NMEA\n"); - gps = new AP_GPS_NMEA(_port); - break; - } - } - return(gps); -} - -int -AP_GPS_Auto::_getc(void) -{ - while (0 == _port->available()) - ; - return(_port->read()); -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h deleted file mode 100644 index a259add7c0..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h +++ /dev/null @@ -1,52 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// Auto-detecting pseudo-GPS driver -// - -#ifndef AP_GPS_Auto_h -#define AP_GPS_Auto_h - -#include -#include - -class AP_GPS_Auto : public GPS -{ -public: - /// Constructor - /// - /// @note The stream is expected to be set up and configured for the - /// correct bitrate before ::init is called. - /// - /// @param port Stream connected to the GPS module. - /// @param ptr Pointer to a GPS * that will be fixed up by ::init - /// when the GPS type has been detected. - /// - AP_GPS_Auto(FastSerial *port, GPS **gps) : - _port(port), - _gps(gps) - {}; - - void init(void); - - /// Detect and initialise the attached GPS unit. Returns a - /// pointer to the allocated & initialised GPS driver. - /// - void update(void); - -private: - /// Serial port connected to the GPS. - FastSerial *_port; - - /// global GPS driver pointer, updated by auto-detection - /// - GPS **_gps; - - /// low-level auto-detect routine - /// - GPS *_detect(void); - - /// fetch a character from the port - /// - int _getc(void); -}; -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp deleted file mode 100644 index 53195ef75f..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp +++ /dev/null @@ -1,224 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -/* - GPS_MTK.cpp - Ublox GPS library for Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) - - This library 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. - - GPS configuration : Costum protocol - Baud rate : 38400 - - Methods: - init() : GPS initialization - update() : Call this funcion as often as you want to ensure you read the incomming gps data - - Properties: - lattitude : lattitude * 10000000 (long value) - longitude : longitude * 10000000 (long value) - altitude : altitude * 100 (meters) (long value) - ground_speed : Speed (m/s) * 100 (long value) - ground_course : Course (degrees) * 100 (long value) - new_data : 1 when a new data is received. - You need to write a 0 to new_data when you read the data - fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix. - -*/ -#include "AP_GPS_IMU.h" -#include "WProgram.h" - - -// Constructors //////////////////////////////////////////////////////////////// -AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s) -{ -} - - -// Public Methods ////////////////////////////////////////////////////////////// -void AP_GPS_IMU::init(void) -{ - // we expect the stream to already be open at the corret bitrate -} - -// optimization : This code doesn't wait for data. It only proccess the data available. -// We can call this function on the main loop (50Hz loop) -// If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info. -void AP_GPS_IMU::update(void) -{ - byte data; - int numc = 0; - static byte message_num = 0; - - numc = _port->available(); - - if (numc > 0){ - for (int i=0;iread(); - - switch(step){ //Normally we start from zero. This is a state machine - case 0: - if(data == 0x44) // IMU sync char 1 - step++; //OH first data packet is correct, so jump to the next step - break; - - case 1: - if(data == 0x49) // IMU sync char 2 - step++; //ooh! The second data packet is correct, jump to the step 2 - else - step=0; //Nop, is not correct so restart to step zero and try again. - break; - - case 2: - if(data == 0x59) // IMU sync char 3 - step++; //ooh! The second data packet is correct, jump to the step 2 - else - step=0; //Nop, is not correct so restart to step zero and try again. - break; - - case 3: - if(data == 0x64) // IMU sync char 4 - step++; //ooh! The second data packet is correct, jump to the step 2 - else - step=0; //Nop, is not correct so restart to step zero and try again. - break; - - case 4: - payload_length = data; - checksum(payload_length); - step++; - if (payload_length > 28){ - step = 0; //Bad data, so restart to step zero and try again. - payload_counter = 0; - ck_a = 0; - ck_b = 0; - //payload_error_count++; - } - break; - - case 5: - message_num = data; - checksum(data); - step++; - break; - - case 6: // Payload data read... - // We stay in this state until we reach the payload_length - buffer[payload_counter] = data; - checksum(data); - payload_counter++; - if (payload_counter >= payload_length) { - step++; - } - break; - - case 7: - GPS_ck_a = data; // First checksum byte - step++; - break; - - case 8: - GPS_ck_b = data; // Second checksum byte - - // We end the IMU/GPS read... - // Verify the received checksum with the generated checksum.. - if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) { - if (message_num == 0x02) { - join_data(); - } else if (message_num == 0x03) { - GPS_join_data(); - } else if (message_num == 0x04) { - join_data_xplane(); - } else if (message_num == 0x0a) { - //PERF_join_data(); - } else { - _error("Invalid message number = %d\n", (int)message_num); - } - } else { - _error("XXX Checksum error\n"); //bad checksum - //imu_checksum_error_count++; - } - // Variable initialization - step = 0; - payload_counter = 0; - ck_a = 0; - ck_b = 0; - break; - } - }// End for... - } -} - -/**************************************************************** - * - ****************************************************************/ - -void AP_GPS_IMU::join_data(void) -{ - //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. - //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. - - //Storing IMU roll - roll_sensor = *(int *)&buffer[0]; - - //Storing IMU pitch - pitch_sensor = *(int *)&buffer[2]; - - //Storing IMU heading (yaw) - ground_course = *(int *)&buffer[4]; - imu_ok = true; -} - -void AP_GPS_IMU::join_data_xplane() -{ - //Storing IMU roll - roll_sensor = *(int *)&buffer[0]; - - - //Storing IMU pitch - pitch_sensor = *(int *)&buffer[2]; - - //Storing IMU heading (yaw) - ground_course = *(unsigned int *)&buffer[4]; - - //Storing airspeed - airspeed = *(int *)&buffer[6]; - - imu_ok = true; - -} - -void AP_GPS_IMU::GPS_join_data(void) -{ - longitude = *(long *)&buffer[0]; // degrees * 10e7 - latitude = *(long *)&buffer[4]; - - //Storing GPS Height above the sea level - altitude = (long)*(int *)&buffer[8] * 10; - - //Storing Speed - speed_3d = ground_speed = (float)*(int *)&buffer[10]; - - //We skip the gps ground course because we use yaw value from the IMU for ground course - time = *(long *)&buffer[14]; - - imu_health = buffer[15]; - - new_data = true; - fix = true; -} - - - -/**************************************************************** - * - ****************************************************************/ -// checksum algorithm -void AP_GPS_IMU::checksum(byte data) -{ - ck_a += data; - ck_b += ck_a; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h deleted file mode 100644 index 0e3f1d294f..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h +++ /dev/null @@ -1,44 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#ifndef AP_GPS_IMU_h -#define AP_GPS_IMU_h - -#include -#define MAXPAYLOAD 32 - -class AP_GPS_IMU : public GPS -{ - public: - - // Methods - AP_GPS_IMU(Stream *s); - void init(); - void update(); - - // Properties - long roll_sensor; // how much we're turning in deg * 100 - long pitch_sensor; // our angle of attack in deg * 100 - int airspeed; - float imu_health; - uint8_t imu_ok; - - private: - // Packet checksums - uint8_t ck_a; - uint8_t ck_b; - uint8_t GPS_ck_a; - uint8_t GPS_ck_b; - - uint8_t step; - uint8_t msg_class; - uint8_t message_num; - uint8_t payload_length; - uint8_t payload_counter; - uint8_t buffer[MAXPAYLOAD]; - - void join_data(); - void join_data_xplane(); - void GPS_join_data(); - void checksum(unsigned char data); -}; - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp deleted file mode 100644 index db281390c0..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp +++ /dev/null @@ -1,146 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com -// -// This library 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. -// -// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" -// - -#include "AP_GPS_MTK.h" -#include "WProgram.h" - -// Constructors //////////////////////////////////////////////////////////////// -AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s) -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void AP_GPS_MTK::init(void) -{ - _port->flush(); - // initialize serial port for binary protocol use - // XXX should assume binary, let GPS_AUTO handle dynamic config? - _port->print(MTK_SET_BINARY); - - // set 4Hz update rate - _port->print(MTK_OUTPUT_4HZ); -} - -// Process bytes available from the stream -// -// The stream is assumed to contain only our custom message. If it -// contains other messages, and those messages contain the preamble bytes, -// it is possible for this code to become de-synchronised. Without -// buffering the entire message and re-processing it from the top, -// this is unavoidable. -// -// The lack of a standard header length field makes it impossible to skip -// unrecognised messages. -// -void AP_GPS_MTK::update(void) -{ - byte data; - int numc; - - numc = _port->available(); - for (int i = 0; i < numc; i++){ // Process bytes received - - // read the next byte - data = _port->read(); - -restart: - switch(_step){ - - // Message preamble, class, ID detection - // - // If we fail to match any of the expected bytes, we - // reset the state machine and re-consider the failed - // byte as the first byte of the preamble. This - // improves our chances of recovering from a mismatch - // and makes it less likely that we will be fooled by - // the preamble appearing as data in some other message. - // - case 0: - if(PREAMBLE1 == data) - _step++; - break; - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - goto restart; - case 2: - if (MESSAGE_CLASS == data) { - _step++; - _ck_b = _ck_a = data; // reset the checksum accumulators - } else { - _step = 0; // reset and wait for a message of the right class - goto restart; - } - break; - case 3: - if (MESSAGE_ID == data) { - _step++; - _ck_b += (_ck_a += data); - _payload_length = sizeof(diyd_mtk_msg); // prepare to receive our message - _payload_counter = 0; - } else { - _step = 0; - goto restart; - } - break; - - // Receive message data - // - case 4: - _buffer.bytes[_payload_counter++] = data; - _ck_b += (_ck_a += data); - if (_payload_counter == _payload_length) - _step++; - break; - - // Checksum and message processing - // - case 5: - _step++; - if (_ck_a != data) { - _error("GPS_MTK: checksum error\n"); - _step = 0; - } - break; - case 6: - _step = 0; - if (_ck_b != data) { - _error("GPS_MTK: checksum error\n"); - break; - } - _parse_gps(); // Parse the new GPS packet - } - } -} - -// Private Methods -void -AP_GPS_MTK::_parse_gps(void) -{ - fix = (_buffer.msg.fix_type == FIX_3D); - latitude = _swapl(&_buffer.msg.latitude) * 10; - longitude = _swapl(&_buffer.msg.longitude) * 10; - altitude = _swapl(&_buffer.msg.altitude); - ground_speed = _swapl(&_buffer.msg.ground_speed); - ground_course = _swapl(&_buffer.msg.ground_course) / 10000; - num_sats = _buffer.msg.satellites; - - // XXX docs say this is UTC, but our clients expect msToW - time = _swapl(&_buffer.msg.utc_time); - _setTime(); - valid_read = true; - new_data = true; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h deleted file mode 100644 index a69dca60df..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h +++ /dev/null @@ -1,87 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com -// -// This library 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. -// -// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" -// -#ifndef AP_GPS_MTK_h -#define AP_GPS_MTK_h - -#include -#define MAXPAYLOAD 32 - -#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" -#define MTK_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n" - -#define MTK_OUTPUT_1HZ "$PMTK220,1000*1F\r\n" -#define MTK_OUTPUT_2HZ "$PMTK220,500*2B\r\n" -#define MTK_OUTPUT_4HZ "$PMTK220,250*29\r\n" -#define MTK_OTUPUT_5HZ "$PMTK220,200*2C\r\n" -#define MTK_OUTPUT_10HZ "$PMTK220,100*2F\r\n" - -#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n" - -#define SBAS_ON "$PMTK313,1*2E\r\n" -#define SBAS_OFF "$PMTK313,0*2F\r\n" - -#define WAAS_ON "$PSRF151,1*3F\r\n" -#define WAAS_OFF "$PSRF151,0*3E\r\n" - -class AP_GPS_MTK : public GPS { -public: - AP_GPS_MTK(Stream *s); - virtual void init(void); - virtual void update(void); - -private: -#pragma pack(1) - struct diyd_mtk_msg { - int32_t latitude; - int32_t longitude; - int32_t altitude; - int32_t ground_speed; - int32_t ground_course; - uint8_t satellites; - uint8_t fix_type; - uint32_t utc_time; - }; -#pragma pack(pop) - enum diyd_mtk_fix_type { - FIX_NONE = 1, - FIX_2D = 2, - FIX_3D = 3 - }; - - enum diyd_mtk_protocol_bytes { - PREAMBLE1 = 0xb5, - PREAMBLE2 = 0x62, - MESSAGE_CLASS = 1, - MESSAGE_ID = 5 - }; - - // Packet checksum accumulators - uint8_t _ck_a; - uint8_t _ck_b; - - // State machine state - uint8_t _step; - uint8_t _payload_length; - uint8_t _payload_counter; - - // Receive buffer - union { - diyd_mtk_msg msg; - uint8_t bytes[]; - } _buffer; - - // Buffer parse & GPS state update - void _parse_gps(); -}; - -#endif // AP_GPS_MTK_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp deleted file mode 100644 index 4f9bf6f6c0..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ /dev/null @@ -1,246 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -/* - GPS_NMEA.cpp - Generic NMEA GPS library for Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - Edits by HappyKillmore - This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1) - - This library 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. - - GPS configuration : NMEA protocol - Baud rate : 38400 - NMEA Sentences : - $GPGGA : Global Positioning System fix Data - $GPVTG : Ttack and Ground Speed - - Methods: - init() : GPS Initialization - update() : Call this funcion as often as you want to ensure you read the incomming gps data - - Properties: - latitude : latitude * 10000000 (long value) - longitude : longitude * 10000000 (long value) - altitude : altitude * 1000 (milimeters) (long value) - ground_speed : Speed (m / s) * 100 (long value) - ground_course : Course (degrees) * 100 (long value) - Type : 2 (This indicate that we are using the Generic NMEA library) - new_data : 1 when a new data is received. - You need to write a 0 to new_data when you read the data - fix : > = 1: GPS FIX, 0: No fix (normal logic) - quality : 0 = No fix - 1 = Bad (Num sats < 5) - 2 = Poor - 3 = Medium - 4 = Good - - NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset) -*/ -#include "AP_GPS_NMEA.h" -#include "WProgram.h" - -// Constructors //////////////////////////////////////////////////////////////// -AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s) -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void -AP_GPS_NMEA::init(void) -{ - //Type = 2; - // initialize serial port for binary protocol use - _port->print(NMEA_OUTPUT_SENTENCES); - _port->print(NMEA_OUTPUT_4HZ); - _port->print(SBAS_ON); - _port->print(DGPS_SBAS); - _port->print(DATUM_GOOGLE); -} - -// This code don´t wait for data, only proccess the data available on serial port -// We can call this function on the main loop (50Hz loop) -// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info. -void -AP_GPS_NMEA::update(void) -{ - char c; - int numc; - int i; - - numc = _port->available(); - - if (numc > 0){ - for (i = 0; i < numc; i++){ - c = _port->read(); - if (c == '$'){ // NMEA Start - bufferidx = 0; - buffer[bufferidx++] = c; - GPS_checksum = 0; - GPS_checksum_calc = true; - continue; - } - if (c == '\r'){ // NMEA End - buffer[bufferidx++] = 0; - parse_nmea_gps(); - } else { - if (bufferidx < (GPS_BUFFERSIZE - 1)){ - if (c == '*') - GPS_checksum_calc = false; // Checksum calculation end - buffer[bufferidx++] = c; - if (GPS_checksum_calc){ - GPS_checksum ^= c; // XOR - } - } else { - bufferidx = 0; // Buffer overflow : restart - } - } - } - } -} - -/**************************************************************** - * - * * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/ -// Private Methods ////////////////////////////////////////////////////////////// -void -AP_GPS_NMEA::parse_nmea_gps(void) -{ - uint8_t NMEA_check; - long aux_deg; - long aux_min; - char *parseptr; - - if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA - if (buffer[bufferidx-4]=='*'){ // Check for the "*" character - NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters - if (GPS_checksum == NMEA_check){ // Checksum validation - //Serial.println("buffer"); - _setTime(); - valid_read = true; - new_data = true; // New GPS Data - parseptr = strchr(buffer, ',')+1; - //parseptr = strchr(parseptr, ',')+1; - time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss - parseptr = strchr(parseptr, ',')+1; - aux_deg = parsedecimal(parseptr, 2); // degrees - aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal - latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5) - parseptr = strchr(parseptr, ',')+1; - if ( * parseptr == 'S') - latitude = -1 * latitude; // South latitudes are negative - parseptr = strchr(parseptr, ',')+1; - // W longitudes are Negative - aux_deg = parsedecimal(parseptr, 3); // degrees - aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal) - longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) - //longitude = -1*longitude; // This Assumes that we are in W longitudes... - parseptr = strchr(parseptr, ',')+1; - if ( * parseptr == 'W') - longitude = -1 * longitude; // West longitudes are negative - parseptr = strchr(parseptr, ',')+1; - fix = parsedecimal(parseptr, 1); - parseptr = strchr(parseptr, ',')+1; - num_sats = parsedecimal(parseptr, 2); - parseptr = strchr(parseptr, ',')+1; - HDOP = parsenumber(parseptr, 1); // HDOP * 10 - parseptr = strchr(parseptr, ',')+1; - altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters - if (fix < 1) - quality = 0; // No FIX - else if(num_sats < 5) - quality = 1; // Bad (Num sats < 5) - else if(HDOP > 30) - quality = 2; // Poor (HDOP > 30) - else if(HDOP > 25) - quality = 3; // Medium (HDOP > 25) - else - quality = 4; // Good (HDOP < 25) - } else { - _error("GPSERR: Checksum error!!\n"); - } - } - } else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG - //Serial.println(buffer); - if (buffer[bufferidx-4]=='*'){ // Check for the "*" character - NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters - if (GPS_checksum == NMEA_check){ // Checksum validation - _setTime(); - valid_read = true; - new_data = true; // New GPS Data - parseptr = strchr(buffer, ',')+1; - ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100 - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100) - //GPS_line = true; - } else { - _error("GPSERR: Checksum error!!\n"); - } - } - } else { - bufferidx = 0; - _error("GPSERR: Bad sentence!!\n"); - } -} - - - // Parse hexadecimal numbers -uint8_t -AP_GPS_NMEA::parseHex(char c) { - if (c < '0') - return (0); - if (c <= '9') - return (c - '0'); - if (c < 'A') - return (0); - if (c <= 'F') - return ((c - 'A')+10); -} - -// Decimal number parser -long -AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) { - long d = 0; - uint8_t i; - - i = num_car; - while ((str[0] != 0) && (i > 0)) { - if ((str[0] > '9') || (str[0] < '0')) - return d; - d *= 10; - d += str[0] - '0'; - str++; - i--; - } - return d; -} - -// Function to parse fixed point numbers (numdec=number of decimals) -long -AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) { - long d = 0; - uint8_t ndec = 0; - - while (str[0] != 0) { - if (str[0] == '.'){ - ndec = 1; - } else { - if ((str[0] > '9') || (str[0] < '0')) - return d; - d *= 10; - d += str[0] - '0'; - if (ndec > 0) - ndec++; - if (ndec > numdec) // we reach the number of decimals... - return d; - } - str++; - } - return d; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h deleted file mode 100644 index b7b46bee15..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h +++ /dev/null @@ -1,60 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#ifndef AP_GPS_NMEA_h -#define AP_GPS_NMEA_h - -#include -#define GPS_BUFFERSIZE 120 - -#define NMEA_OUTPUT_SENTENCES "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GPGGA and GPVTG - -#define NMEA_BAUD_RATE_4800 "$PSRF100,1,4800,8,1,0*0E\r\n" -#define NMEA_BAUD_RATE_9600 "$PSRF100,1,9600,8,1,0*0D\r\n" -#define NMEA_BAUD_RATE_19200 "$PSRF100,1,19200,8,1,0*38\r\n" -#define NMEA_BAUD_RATE_38400 "$PSRF100,1,38400,8,1,0*3D\r\n" -#define NMEA_BAUD_RATE_57600 "$PSRF100,1,57600,8,1,0*36\r\n" - -#define NMEA_OUTPUT_1HZ "$PMTK220,1000*1F\r\n" -#define NMEA_OUTPUT_2HZ "$PMTK220,500*2B\r\n" -#define NMEA_OUTPUT_4HZ "$PMTK220,250*29\r\n" -#define NMEA_OTUPUT_5HZ "$PMTK220,200*2C\r\n" -#define NMEA_OUTPUT_10HZ "$PMTK220,100*2F\r\n" - -#define SBAS_ON "$PMTK313,1*2E\r\n" -#define SBAS_OFF "$PMTK313,0*2F\r\n" - -#define WAAS_ON "$PSRF151,1*3F\r\n" -#define WAAS_OFF "$PSRF151,0*3E\r\n" - -#define DGPS_OFF "$PMTK301,0*2C\r\n" -#define DGPS_RTCM "$PMTK301,1*2D\r\n" -#define DGPS_SBAS "$PMTK301,2*2E\r\n" - -#define DATUM_GOOGLE "$PMTK330,0*2E\r\n" - -class AP_GPS_NMEA : public GPS -{ - public: - // Methods - AP_GPS_NMEA(Stream *s); - void init(); - void update(); - - // Properties - uint8_t quality; // GPS Signal quality - int HDOP; // HDOP - - private: - // Internal variables - uint8_t GPS_checksum; - uint8_t GPS_checksum_calc; - char buffer[GPS_BUFFERSIZE]; - int bufferidx; - - void parse_nmea_gps(void); - uint8_t parseHex(char c); - long parsedecimal(char *str,uint8_t num_car); - long parsenumber(char *str,uint8_t numdec); - -}; - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h deleted file mode 100644 index 23f242b59c..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h +++ /dev/null @@ -1,13 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#ifndef AP_GPS_None_h -#define AP_GPS_None_h - -#include - -class AP_GPS_None : public GPS -{ - virtual void init(void) {}; - virtual void update(void) {}; -}; -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp deleted file mode 100644 index f1fe5e9f5c..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ /dev/null @@ -1,193 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// SiRF Binary GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith. -// -// This library 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. -// - -#include "AP_GPS_SIRF.h" -#include "WProgram.h" - -// Initialisation messages -// -// Turn off all messages except for 0x29. -// -// XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them. -// -static uint8_t init_messages[] = { - 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3, - 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 -}; - -// Constructors //////////////////////////////////////////////////////////////// -AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s) -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void AP_GPS_SIRF::init(void) -{ - _port->flush(); - - // For modules that default to something other than SiRF binary, - // the module-specific subclass should take care of switching to binary mode - // before calling us. - - // send SiRF binary setup messages - _port->write(init_messages, sizeof(init_messages)); -} - -// Process bytes available from the stream -// -// The stream is assumed to contain only messages we recognise. If it -// contains other messages, and those messages contain the preamble -// bytes, it is possible for this code to fail to synchronise to the -// stream immediately. Without buffering the entire message and -// re-processing it from the top, this is unavoidable. The parser -// attempts to avoid this when possible. -// -void AP_GPS_SIRF::update(void) -{ - byte data; - int numc; - - numc = _port->available(); - while(numc--) { - - // read the next byte - data = _port->read(); - - switch(_step){ - - // Message preamble detection - // - // If we fail to match any of the expected bytes, we reset - // the state machine and re-consider the failed byte as - // the first byte of the preamble. This improves our - // chances of recovering from a mismatch and makes it less - // likely that we will be fooled by the preamble appearing - // as data in some other message. - // - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - // FALLTHROUGH - case 0: - if(PREAMBLE1 == data) - _step++; - break; - - // Message length - // - // We always collect the length so that we can avoid being - // fooled by preamble bytes in messages. - // - case 2: - _step++; - _payload_length = (uint16_t)data << 8; - break; - case 3: - _step++; - _payload_length |= data; - _payload_counter = 0; - _checksum = 0; - break; - - // Message header processing - // - // We sniff the message ID to determine whether we are going - // to gather the message bytes or just discard them. - // - case 4: - _step++; - _accumulate(data); - _payload_length--; - _gather = false; - switch(data) { - case MSG_GEONAV: - if (_payload_length == sizeof(sirf_geonav)) { - _gather = true; - _msg_id = data; - } - break; - } - break; - - // Receive message data - // - // Note that we are effectively guaranteed by the protocol - // that the checksum and postamble cannot be mistaken for - // the preamble, so if we are discarding bytes in this - // message when the payload is done we return directly - // to the preamble detector rather than bothering with - // the checksum logic. - // - case 5: - if (_gather) { // gather data if requested - _accumulate(data); - _buffer.bytes[_payload_counter] = data; - if (++_payload_counter == _payload_length) - _step++; - } else { - if (++_payload_counter == _payload_length) - _step = 0; - } - break; - - // Checksum and message processing - // - case 6: - _step++; - if ((_checksum >> 8) != data) { - _error("GPS_SIRF: checksum error\n"); - _step = 0; - } - break; - case 7: - _step = 0; - if ((_checksum & 0xff) != data) { - _error("GPS_SIRF: checksum error\n"); - break; - } - if (_gather) { - _parse_gps(); // Parse the new GPS packet - } - } - } -} - -void -AP_GPS_SIRF::_parse_gps(void) -{ - switch(_msg_id) { - case MSG_GEONAV: - time = _swapl(&_buffer.nav.time); - //fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK)); - fix = (0 == _buffer.nav.fix_invalid); - latitude = _swapl(&_buffer.nav.latitude); - longitude = _swapl(&_buffer.nav.longitude); - altitude = _swapl(&_buffer.nav.altitude_msl); - ground_speed = _swapi(&_buffer.nav.ground_speed); - // at low speeds, ground course wanders wildly; suppress changes if we are not moving - if (ground_speed > 50) - ground_course = _swapi(&_buffer.nav.ground_course); - num_sats = _buffer.nav.satellites; - _setTime(); - valid_read = 1; - break; - } - new_data = true; -} - -void -AP_GPS_SIRF::_accumulate(uint8_t val) -{ - _checksum = (_checksum + val) & 0x7fff; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h deleted file mode 100644 index ce3ff454be..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h +++ /dev/null @@ -1,96 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// SiRF Binary GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith. -// -// This library 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. -// -#ifndef AP_GPS_SIRF_h -#define AP_GPS_SIRF_h - -#include - -#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C" - -class AP_GPS_SIRF : public GPS { -public: - AP_GPS_SIRF(Stream *s); - - virtual void init(); - virtual void update(); - -private: -#pragma pack(1) - struct sirf_geonav { - uint16_t fix_invalid; - uint16_t fix_type; - uint16_t week; - uint32_t time; - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t minute; - uint16_t second; - uint32_t satellites_used; - int32_t latitude; - int32_t longitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - int8_t map_datum; - int16_t ground_speed; - int16_t ground_course; - int16_t res1; - int16_t climb_rate; - uint16_t heading_rate; - uint32_t horizontal_position_error; - uint32_t vertical_position_error; - uint32_t time_error; - int16_t horizontal_velocity_error; - int32_t clock_bias; - uint32_t clock_bias_error; - int32_t clock_drift; - uint32_t clock_drift_error; - uint32_t distance; - uint16_t distance_error; - uint16_t heading_error; - uint8_t satellites; - uint8_t hdop; - uint8_t mode_info; - }; -#pragma pack(pop) - enum sirf_protocol_bytes { - PREAMBLE1 = 0xa0, - PREAMBLE2 = 0xa2, - POSTAMBLE1 = 0xb0, - POSTAMBLE2 = 0xb3, - MSG_GEONAV = 0x29 - }; - enum sirf_fix_type { - FIX_3D = 0x6, - FIX_MASK = 0x7 - }; - - - // State machine state - uint8_t _step; - uint16_t _checksum; - bool _gather; - uint16_t _payload_length; - uint16_t _payload_counter; - uint8_t _msg_id; - - // Message buffer - union { - sirf_geonav nav; - uint8_t bytes[]; - } _buffer; - - void _parse_gps(void); - void _accumulate(uint8_t val); -}; - -#endif // AP_GPS_SIRF_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp deleted file mode 100644 index cd2541d118..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// u-blox UBX GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com -// -// This library 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. -// - -#include "AP_GPS_UBLOX.h" -#include "WProgram.h" - -// Constructors //////////////////////////////////////////////////////////////// - -AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s) -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// - -void AP_GPS_UBLOX::init(void) -{ - // XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the - // right reporting configuration. - - _port->flush(); -} - -// Process bytes available from the stream -// -// The stream is assumed to contain only messages we recognise. If it -// contains other messages, and those messages contain the preamble -// bytes, it is possible for this code to fail to synchronise to the -// stream immediately. Without buffering the entire message and -// re-processing it from the top, this is unavoidable. The parser -// attempts to avoid this when possible. -// -void AP_GPS_UBLOX::update(void) -{ - byte data; - int numc; - - numc = _port->available(); - for (int i = 0; i < numc; i++){ // Process bytes received - - // read the next byte - data = _port->read(); - - switch(_step){ - - // Message preamble detection - // - // If we fail to match any of the expected bytes, we reset - // the state machine and re-consider the failed byte as - // the first byte of the preamble. This improves our - // chances of recovering from a mismatch and makes it less - // likely that we will be fooled by the preamble appearing - // as data in some other message. - // - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - // FALLTHROUGH - case 0: - if(PREAMBLE1 == data) - _step++; - break; - - // Message header processing - // - // We sniff the class and message ID to decide whether we - // are going to gather the message bytes or just discard - // them. - // - // We always collect the length so that we can avoid being - // fooled by preamble bytes in messages. - // - case 2: - _step++; - if (CLASS_NAV == data) { - _gather = true; // class is interesting, maybe gather - _ck_b = _ck_a = data; // reset the checksum accumulators - } else { - _error("ignoring class 0x%x\n", (int)data); - _gather = false; // class is not interesting, discard - } - break; - case 3: - _step++; - _ck_b += (_ck_a += data); // checksum byte - _msg_id = data; - if (_gather) { // if class was interesting - switch(data) { - case MSG_POSLLH: // message is interesting - _expect = sizeof(ubx_nav_posllh); - break; - case MSG_STATUS: - _expect = sizeof(ubx_nav_status); - break; - case MSG_SOL: - _expect = sizeof(ubx_nav_solution); - break; - case MSG_VELNED: - _expect = sizeof(ubx_nav_velned); - break; - default: - _error("ignoring message 0x%x\n", (int)data); - _gather = false; // message is not interesting - } - } - break; - case 4: - _step++; - _ck_b += (_ck_a += data); // checksum byte - _payload_length = data; // payload length low byte - break; - case 5: - _step++; - _ck_b += (_ck_a += data); // checksum byte - _payload_length += (uint16_t)data; // payload length high byte - _payload_counter = 0; // prepare to receive payload - if (_payload_length != _expect) { - _error("payload %d expected %d\n", _payload_length, _expect); - _gather = false; - } - break; - - // Receive message data - // - case 6: - _ck_b += (_ck_a += data); // checksum byte - if (_gather) // gather data if requested - _buffer.bytes[_payload_counter] = data; - if (++_payload_counter == _payload_length) - _step++; - break; - - // Checksum and message processing - // - case 7: - _step++; - if (_ck_a != data) { - _error("GPS_UBLOX: checksum error\n"); - _step = 0; - } - break; - case 8: - _step = 0; - if (_ck_b != data) { - _error("GPS_UBLOX: checksum error\n"); - break; - } - if (_gather) - _parse_gps(); // Parse the new GPS packet - } - } -} - -// Private Methods ///////////////////////////////////////////////////////////// - -void -AP_GPS_UBLOX::_parse_gps(void) -{ - switch (_msg_id) { - case MSG_POSLLH: - time = _buffer.posllh.time; - longitude = _buffer.posllh.longitude; - latitude = _buffer.posllh.latitude; - altitude = _buffer.posllh.altitude_msl / 10; - break; - case MSG_STATUS: - fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); - break; - case MSG_SOL: - fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); - num_sats = _buffer.solution.satellites; - break; - case MSG_VELNED: - speed_3d = _buffer.velned.speed_3d; // cm/s - ground_speed = _buffer.velned.speed_2d; // cm/s - ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100 - break; - } - _setTime(); - valid_read = 1; - new_data = 1; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h deleted file mode 100644 index ce905a85e8..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h +++ /dev/null @@ -1,124 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// u-blox UBX GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com -// -// This library 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. -// -#ifndef AP_GPS_UBLOX_h -#define AP_GPS_UBLOX_h - -#include - -#define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26" - -class AP_GPS_UBLOX : public GPS -{ -public: - // Methods - AP_GPS_UBLOX(Stream *s = NULL); - void init(void); - void update(); - -private: - // u-blox UBX protocol essentials -#pragma pack(1) - struct ubx_nav_posllh { - uint32_t time; // GPS msToW - int32_t longitude; - int32_t latitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; - }; - struct ubx_nav_status { - uint32_t time; // GPS msToW - uint8_t fix_type; - uint8_t fix_status; - uint8_t differential_status; - uint8_t res; - uint32_t time_to_first_fix; - uint32_t uptime; // milliseconds - }; - struct ubx_nav_solution { - uint32_t time; - int32_t time_nsec; - int16_t week; - uint8_t fix_type; - uint8_t fix_status; - int32_t ecef_x; - int32_t ecef_y; - int32_t ecef_z; - uint32_t position_accuracy_3d; - int32_t ecef_x_velocity; - int32_t ecef_y_velocity; - int32_t ecef_z_velocity; - uint32_t speed_accuracy; - uint16_t position_DOP; - uint8_t res; - uint8_t satellites; - uint32_t res2; - }; - struct ubx_nav_velned { - uint32_t time; // GPS msToW - int32_t ned_north; - int32_t ned_east; - int32_t ned_down; - uint32_t speed_3d; - uint32_t speed_2d; - int32_t heading_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; - }; -#pragma pack(pop) - enum ubs_protocol_bytes { - PREAMBLE1 = 0xb5, - PREAMBLE2 = 0x62, - CLASS_NAV = 0x1, - MSG_POSLLH = 0x2, - MSG_STATUS = 0x3, - MSG_SOL = 0x6, - MSG_VELNED = 0x12 - }; - enum ubs_nav_fix_type { - FIX_NONE = 0, - FIX_DEAD_RECKONING = 1, - FIX_2D = 2, - FIX_3D = 3, - FIX_GPS_DEAD_RECKONING = 4, - FIX_TIME = 5 - }; - enum ubx_nav_status_bits { - NAV_STATUS_FIX_VALID = 1 - }; - - // Packet checksum accumulators - uint8_t _ck_a; - uint8_t _ck_b; - - // State machine state - uint8_t _step; - uint8_t _msg_id; - bool _gather; - uint16_t _expect; - uint16_t _payload_length; - uint16_t _payload_counter; - - // Receive buffer - union { - ubx_nav_posllh posllh; - ubx_nav_status status; - ubx_nav_solution solution; - ubx_nav_velned velned; - uint8_t bytes[]; - } _buffer; - - // Buffer parse & GPS state update - void _parse_gps(); -}; - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp deleted file mode 100644 index 89bfefa1a0..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include "GPS.h" -#include "WProgram.h" -#include - -int -GPS::status(void) -{ - if (millis() - _lastTime >= 500){ - return 0; - } else if (fix == 0) { - return 1; - } else { - return 2; - } -} - -void -GPS::_setTime(void) -{ - _lastTime = millis(); -} - -void -GPS::_error(const char *fmt, ...) -{ - va_list ap; - - if (print_errors && stderr) { - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - } -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h deleted file mode 100644 index 219254ba41..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h +++ /dev/null @@ -1,150 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file GPS.h -/// @brief Interface definition for the various GPS drivers. - -#ifndef GPS_h -#define GPS_h - -#include -#include - -/// @class GPS -/// @brief Abstract base class for GPS receiver drivers. -class GPS -{ -public: - - /// Constructor - /// - /// @note The stream is expected to be set up and configured for the - /// correct bitrate before ::init is called. - /// - /// @param s Stream connected to the GPS module. If NULL, assumed - /// to be set up at ::init time. Support for setting - /// the port in the ctor for backwards compatibility. - /// - GPS(Stream *s = NULL) : _port(s) {}; - - /// Startup initialisation. - /// - /// This routine performs any one-off initialisation required to set the - /// GPS up for use. - /// - /// Must be implemented by the GPS driver. - /// - /// @param s Stream connected to the GPS module. If NULL, assumed to - /// have been set up at constructor time. - /// - virtual void init(void) = 0; - - /// Update GPS state based on possible bytes received from the module. - /// - /// This routine must be called periodically to process incoming data. - /// - /// Must be implemented by the GPS driver. - /// - virtual void update(void) = 0; - - /// Query GPS status - /// - /// The 'valid message' status indicates that a recognised message was - /// received from the GPS within the last 500ms. - /// - /// @todo should probably return an enumeration here. - /// - /// @return 0 No GPS connected/detected - /// @return 1 Receiving valid GPS messages but no lock - /// @return 2 Receiving valid messages and locked - /// - int status(void); - - // Properties - long time; ///< GPS time in milliseconds from the start of the week - long latitude; ///< latitude in degrees * 10,000,000 - long longitude; ///< longitude in degrees * 10,000,000 - long altitude; ///< altitude in cm - long ground_speed; ///< ground speed in cm/sec - long ground_course; ///< ground course in 100ths of a degree - long speed_3d; ///< 3D speed in cm/sec (not always available) - uint8_t num_sats; ///< Number of visible satelites - - /// Set to true when new data arrives. A client may set this - /// to false in order to avoid processing data they have - /// already seen. - bool new_data; - - // Deprecated properties - bool fix; ///< true if we have a position fix (use ::status instead) - bool valid_read; ///< true if we have seen data from the GPS (use ::status instead) - - // Debug support - bool print_errors; ///< if true, errors will be printed to stderr - -protected: - Stream *_port; ///< stream port the GPS is attached to - unsigned long _lastTime; ///< Timer for lost connection - - /// reset the last-message-received timer used by ::status - /// - void _setTime(void); - - /// perform an endian swap on a long - /// - /// @param bytes pointer to a buffer containing bytes representing a - /// long in the wrong byte order - /// @returns endian-swapped value - /// - long _swapl(const void *bytes); - - /// perform an endian swap on an int - /// - /// @param bytes pointer to a buffer containing bytes representing an - /// int in the wrong byte order - /// @returns endian-swapped value - int _swapi(const void *bytes); - - /// emit an error message - /// - /// based on the value of print_errors, emits the printf-formatted message - /// in msg,... to stderr - /// - /// @param fmt printf-like format string - /// - void _error(const char *msg, ...); - -}; - -inline long -GPS::_swapl(const void *bytes) -{ - const uint8_t *b = (const uint8_t *)bytes; - union { - long v; - uint8_t b[4]; - } u; - - u.b[0] = b[3]; - u.b[1] = b[2]; - u.b[2] = b[1]; - u.b[3] = b[0]; - - return(u.v); -} - -inline int16_t -GPS::_swapi(const void *bytes) -{ - const uint8_t *b = (const uint8_t *)bytes; - union { - int16_t v; - uint8_t b[2]; - } u; - - u.b[0] = b[1]; - u.b[1] = b[0]; - - return(u.v); -} - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde deleted file mode 100644 index 8ffadd5680..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde +++ /dev/null @@ -1,58 +0,0 @@ -/* - Example of GPS 406 library. - Code by Jordi Munoz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) -*/ - -#include -#include -#include - -FastSerialPort0(Serial); -FastSerialPort1(Serial1); - -AP_GPS_406 gps(&Serial1); -#define T6 1000000 -#define T7 10000000 - -void setup() -{ - tone(A6, 1000, 200); - - Serial.begin(38400, 16, 128); - Serial1.begin(57600, 128, 16); - stderr = stdout; - gps.print_errors = true; - - Serial.println("GPS 406 library test"); - gps.init(); // GPS Initialization - delay(1000); -} -void loop() -{ - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have readed the data - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde deleted file mode 100644 index d60057e532..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ /dev/null @@ -1,59 +0,0 @@ -// -// Test for AP_GPS_AUTO -// - -#include -#include -#include - -FastSerialPort0(Serial); -FastSerialPort1(Serial1); - -GPS *gps; -AP_GPS_Auto GPS(&Serial1, &gps); - -#define T6 1000000 -#define T7 10000000 - -void * operator new(size_t size) -{ - return(calloc(size, 1)); -} - -void setup() -{ - Serial.begin(38400); - Serial1.begin(38400); - - Serial.println("GPS AUTO library test"); - gps = &GPS; - gps->init(); - delay(1000); -} -void loop() -{ - delay(20); - gps->update(); - if (gps->new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps->latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps->longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps->altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps->ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps->ground_course / 100.0, DEC); - Serial.print(" SAT:"); - Serial.print(gps->num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps->fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps->time, DEC); - Serial.println(); - gps->new_data = 0; - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde deleted file mode 100644 index bda3b85229..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde +++ /dev/null @@ -1,56 +0,0 @@ -/* - Example of GPS MTK library. - Code by Jordi Munoz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) -*/ - -#include -#include -#include - -FastSerialPort0(Serial); -FastSerialPort1(Serial1); - -AP_GPS_MTK gps(&Serial1); -#define T6 1000000 -#define T7 10000000 - -void setup() -{ - Serial.begin(38400); - Serial1.begin(38400); - stderr = stdout; - gps.print_errors = true; - - Serial.println("GPS MTK library test"); - gps.init(); // GPS Initialization - delay(1000); -} -void loop() -{ - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100.0, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have readed the data - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde deleted file mode 100644 index fd071541a4..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde +++ /dev/null @@ -1,57 +0,0 @@ -/* - Example of GPS NMEA library. - Code by Jordi Munoz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) -*/ - -#include -#include -#include - -FastSerialPort0(Serial); -FastSerialPort1(Serial1); - -AP_GPS_NMEA gps(&Serial1); -#define T6 1000000 -#define T7 10000000 - -void setup() -{ - Serial.begin(38400); - Serial1.begin(38400); - stderr = stdout; - gps.print_errors = true; - - Serial.println("GPS NMEA library test"); - gps.init(); // GPS Initialization - delay(1000); -} - -void loop() -{ - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have read the data - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde deleted file mode 100644 index 43eb239495..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde +++ /dev/null @@ -1,56 +0,0 @@ -/* - Example of GPS UBlox library. - Code by Jordi Munoz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) -*/ - -#include -#include -#include - -FastSerialPort0(Serial); -FastSerialPort1(Serial1); - -AP_GPS_UBLOX gps(&Serial1); -#define T6 1000000 -#define T7 10000000 - -void setup() -{ - Serial.begin(38400); - Serial1.begin(38400); - stderr = stdout; - gps.print_errors = true; - - Serial.println("GPS UBLOX library test"); - gps.init(); // GPS Initialization - delay(1000); -} -void loop() -{ - delay(20); - gps.update(); - if (gps.new_data){ - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100.0, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); - gps.new_data = 0; // We have readed the data - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h deleted file mode 100644 index b94ed1c696..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/AP_Math.h +++ /dev/null @@ -1,7 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Assorted useful math operations for ArduPilot(Mega) - -#include "vector2.h" -#include "vector3.h" -#include "matrix3.h" diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/keywords.txt deleted file mode 100644 index a5b7938f47..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/keywords.txt +++ /dev/null @@ -1,29 +0,0 @@ -Vector2 KEYWORD1 -Vector2i KEYWORD1 -Vector2ui KEYWORD1 -Vector2l KEYWORD1 -Vector2ul KEYWORD1 -Vector2f KEYWORD1 -Vector3 KEYWORD1 -Vector3i KEYWORD1 -Vector3ui KEYWORD1 -Vector3l KEYWORD1 -Vector3ul KEYWORD1 -Vector3f KEYWORD1 -Matrix3 KEYWORD1 -Matrix3i KEYWORD1 -Matrix3ui KEYWORD1 -Matrix3l KEYWORD1 -Matrix3ul KEYWORD1 -Matrix3f KEYWORD1 -length_squared KEYWORD2 -length KEYWORD2 -normalize KEYWORD2 -normalized KEYWORD2 -reflect KEYWORD2 -project KEYWORD2 -projected KEYWORD2 -angle KEYWORD2 -angle_normalized KEYWORD2 -rotate KEYWORD2 -rotated KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h deleted file mode 100644 index 51fd144c75..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/matrix3.h +++ /dev/null @@ -1,134 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Copyright 2010 Michael Smith, all rights reserved. - -// This library 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. - -// Inspired by: -/**************************************** - * 3D Vector Classes - * By Bill Perone (billperone@yahoo.com) - */ - -// -// 3x3 matrix implementation. -// -// Note that the matrix is organised in row-normal form (the same as -// applies to array indexing). -// -// In addition to the template, this header defines the following types: -// -// Matrix3i 3x3 matrix of signed integers -// Matrix3ui 3x3 matrix of unsigned integers -// Matrix3l 3x3 matrix of signed longs -// Matrix3ul 3x3 matrix of unsigned longs -// Matrix3f 3x3 matrix of signed floats -// - -#ifndef MATRIX3_H -#define MATRIX3_H - -#include "vector3.h" - -// 3x3 matrix with elements of type T -template -class Matrix3 { -public: - - // Vectors comprising the rows of the matrix - Vector3 a, b, c; - - // trivial ctor - Matrix3() {} - - // setting ctor - Matrix3(const Vector3 a0, const Vector3 b0, const Vector3 c0): a(a0), b(b0), c(c0) {} - - // setting ctor - Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz): a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {} - - // function call operator - void operator () (const Vector3 a0, const Vector3 b0, const Vector3 c0) - { a = a0; b = b0; c = c0; } - - // test for equality - bool operator == (const Matrix3 &m) - { return (a==m.a && b==m.b && c==m.c); } - - // test for inequality - bool operator != (const Matrix3 &m) - { return (a!=m.a || b!=m.b || c!=m.c); } - - // negation - Matrix3 operator - (void) const - { return Matrix3(-a,-b,-c); } - - // addition - Matrix3 operator + (const Matrix3 &m) const - { return Matrix3(a+m.a, b+m.b, c+m.c); } - Matrix3 &operator += (const Matrix3 &m) - { return *this = *this + m; } - - // subtraction - Matrix3 operator - (const Matrix3 &m) const - { return Matrix3(a-m.a, b-m.b, c-m.c); } - Matrix3 &operator -= (const Matrix3 &m) - { return *this = *this - m; } - - // uniform scaling - Matrix3 operator * (const T num) const - { return Matrix3(a*num, b*num, c*num); } - Matrix3 &operator *= (const T num) - { return *this = *this * num; } - Matrix3 operator / (const T num) const - { return Matrix3(a/num, b/num, c/num); } - Matrix3 &operator /= (const T num) - { return *this = *this / num; } - - // multiplication by a vector - Vector3 operator *(const Vector3 &v) const - { - return Vector3(a.x * v.x + a.y * v.y + a.z * v.z, - b.x * v.x + b.y * v.y + b.z * v.z, - c.x * v.x + c.y * v.y + c.z * v.z); - } - - // multiplication by another Matrix3 - Matrix3 operator *(const Matrix3 &m) const - { - Matrix3 temp (Vector3(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x, - a.x * m.a.y + a.y * m.b.y + a.z * m.c.y, - a.x * m.a.z + a.y * m.b.z + a.z * m.c.z), - Vector3(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x, - b.x * m.a.y + b.y * m.b.y + b.z * m.c.y, - b.x * m.a.z + b.y * m.b.z + b.z * m.c.z), - Vector3(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x, - c.x * m.a.y + c.y * m.b.y + c.z * m.c.y, - c.x * m.a.z + c.y * m.b.z + c.z * m.c.z)); - return temp; - } - Matrix3 &operator *=(const Matrix3 &m) - { return *this = *this * m; } - - // transpose the matrix - Matrix3 transposed(void) - { - return Matrix3(Vector3(a.x, b.x, c.x), - Vector3(a.y, b.y, c.y), - Vector3(a.z, b.z, c.z)); - } - Matrix3 transpose(void) - { return *this = transposed(); } - -}; - -typedef Matrix3 Matrix3i; -typedef Matrix3 Matrix3ui; -typedef Matrix3 Matrix3l; -typedef Matrix3 Matrix3ul; -typedef Matrix3 Matrix3f; - -#endif // MATRIX3_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector2.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector2.h deleted file mode 100644 index bbe380aeb6..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector2.h +++ /dev/null @@ -1,158 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Copyright 2010 Michael Smith, all rights reserved. - -// This library 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. - -// Derived closely from: -/**************************************** - * 2D Vector Classes - * By Bill Perone (billperone@yahoo.com) - * Original: 9-16-2002 - * Revised: 19-11-2003 - * 18-12-2003 - * 06-06-2004 - * - * © 2003, This code is provided "as is" and you can use it freely as long as - * credit is given to Bill Perone in the application it is used in - ****************************************/ - -#ifndef VECTOR2_H -#define VECTOR2_H - -#include - -template -struct Vector2 -{ - T x, y; - - - // trivial ctor - Vector2() {} - - // setting ctor - Vector2(const T x0, const T y0): x(x0), y(y0) {} - - // function call operator - void operator ()(const T x0, const T y0) - { x= x0; y= y0; } - - // test for equality - bool operator==(const Vector2 &v) - { return (x==v.x && y==v.y); } - - // test for inequality - bool operator!=(const Vector2 &v) - { return (x!=v.x || y!=v.y); } - - // negation - Vector2 operator -(void) const - { return Vector2(-x, -y); } - - // addition - Vector2 operator +(const Vector2 &v) const - { return Vector2(x+v.x, y+v.y); } - - // subtraction - Vector2 operator -(const Vector2 &v) const - { return Vector2(x-v.x, y-v.y); } - - // uniform scaling - Vector2 operator *(const T num) const - { - Vector2 temp(*this); - return temp*=num; - } - - // uniform scaling - Vector2 operator /(const T num) const - { - Vector2 temp(*this); - return temp/=num; - } - - // addition - Vector2 &operator +=(const Vector2 &v) - { - x+=v.x; y+=v.y; - return *this; - } - - // subtraction - Vector2 &operator -=(const Vector2 &v) - { - x-=v.x; y-=v.y; - return *this; - } - - // uniform scaling - Vector2 &operator *=(const T num) - { - x*=num; y*=num; - return *this; - } - - // uniform scaling - Vector2 &operator /=(const T num) - { - x/=num; y/=num; - return *this; - } - - // dot product - T operator *(const Vector2 &v) const - { return x*v.x + y*v.y; } - - // gets the length of this vector squared - T length_squared() const - { return (T)(*this * *this); } - - // gets the length of this vector - T length() const - { return (T)sqrt(*this * *this); } - - // normalizes this vector - void normalize() - { *this/=length(); } - - // returns the normalized vector - Vector2 normalized() const - { return *this/length(); } - - // reflects this vector about n - void reflect(const Vector2 &n) - { - Vector2 orig(*this); - project(n); - *this= *this*2 - orig; - } - - // projects this vector onto v - void project(const Vector2 &v) - { *this= v * (*this * v)/(v*v); } - - // returns this vector projected onto v - Vector2 projected(const Vector2 &v) - { return v * (*this * v)/(v*v); } - - // computes the angle between 2 arbitrary vectors - static inline T angle(const Vector2 &v1, const Vector2 &v2) - { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } - - // computes the angle between 2 normalized arbitrary vectors - static inline T angle_normalized(const Vector2 &v1, const Vector2 &v2) - { return (T)acosf(v1*v2); } - -}; - -typedef Vector2 Vector2i; -typedef Vector2 Vector2ui; -typedef Vector2 Vector2l; -typedef Vector2 Vector2ul; -typedef Vector2 Vector2f; - -#endif // VECTOR2_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector3.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector3.h deleted file mode 100644 index 7c23a5fc63..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Math/vector3.h +++ /dev/null @@ -1,183 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Copyright 2010 Michael Smith, all rights reserved. - -// This library 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. - -// Derived closely from: -/**************************************** - * 3D Vector Classes - * By Bill Perone (billperone@yahoo.com) - * Original: 9-16-2002 - * Revised: 19-11-2003 - * 11-12-2003 - * 18-12-2003 - * 06-06-2004 - * - * © 2003, This code is provided "as is" and you can use it freely as long as - * credit is given to Bill Perone in the application it is used in - * - * Notes: - * if a*b = 0 then a & b are orthogonal - * a%b = -b%a - * a*(b%c) = (a%b)*c - * a%b = a(cast to matrix)*b - * (a%b).length() = area of parallelogram formed by a & b - * (a%b).length() = a.length()*b.length() * sin(angle between a & b) - * (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0 - * a * (b%c) = volume of parallelpiped formed by a, b, c - * vector triple product: a%(b%c) = b*(a*c) - c*(a*b) - * scalar triple product: a*(b%c) = c*(a%b) = b*(c%a) - * vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c) - * if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0 - * vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0 - * or if the matrix (A) * b = 0 - * - ****************************************/ - -#ifndef VECTOR3_H -#define VECTOR3_H - -#include - -template -class Vector3 -{ -public: - T x, y, z; - - // trivial ctor - Vector3() {} - - // setting ctor - Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {} - - // function call operator - void operator ()(const T x0, const T y0, const T z0) - { x= x0; y= y0; z= z0; } - - // test for equality - bool operator==(const Vector3 &v) - { return (x==v.x && y==v.y && z==v.z); } - - // test for inequality - bool operator!=(const Vector3 &v) - { return (x!=v.x || y!=v.y || z!=v.z); } - - // negation - Vector3 operator -(void) const - { return Vector3(-x,-y,-z); } - - // addition - Vector3 operator +(const Vector3 &v) const - { return Vector3(x+v.x, y+v.y, z+v.z); } - - // subtraction - Vector3 operator -(const Vector3 &v) const - { return Vector3(x-v.x, y-v.y, z-v.z); } - - // uniform scaling - Vector3 operator *(const T num) const - { - Vector3 temp(*this); - return temp*=num; - } - - // uniform scaling - Vector3 operator /(const T num) const - { - Vector3 temp(*this); - return temp/=num; - } - - // addition - Vector3 &operator +=(const Vector3 &v) - { - x+=v.x; y+=v.y; z+=v.z; - return *this; - } - - // subtraction - Vector3 &operator -=(const Vector3 &v) - { - x-=v.x; y-=v.y; z-=v.z; - return *this; - } - - // uniform scaling - Vector3 &operator *=(const T num) - { - x*=num; y*=num; z*=num; - return *this; - } - - // uniform scaling - Vector3 &operator /=(const T num) - { - x/=num; y/=num; z/=num; - return *this; - } - - // dot product - T operator *(const Vector3 &v) const - { return x*v.x + y*v.y + z*v.z; } - - // cross product - Vector3 operator %(const Vector3 &v) const - { - Vector3 temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x); - return temp; - } - - // gets the length of this vector squared - T length_squared() const - { return (T)(*this * *this); } - - // gets the length of this vector - float length() const - { return (T)sqrt(*this * *this); } - - // normalizes this vector - void normalize() - { *this/=length(); } - - // returns the normalized version of this vector - Vector3 normalized() const - { return *this/length(); } - - // reflects this vector about n - void reflect(const Vector3 &n) - { - Vector3 orig(*this); - project(n); - *this= *this*2 - orig; - } - - // projects this vector onto v - void project(const Vector3 &v) - { *this= v * (*this * v)/(v*v); } - - // returns this vector projected onto v - Vector3 projected(const Vector3 &v) - { return v * (*this * v)/(v*v); } - - // computes the angle between 2 arbitrary vectors - static inline T angle(const Vector3 &v1, const Vector3 &v2) - { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } - - // computes the angle between 2 arbitrary normalized vectors - static inline T angle_normalized(const Vector3 &v1, const Vector3 &v2) - { return (T)acosf(v1*v2); } - -}; - -typedef Vector3 Vector3i; -typedef Vector3 Vector3ui; -typedef Vector3 Vector3l; -typedef Vector3 Vector3ul; -typedef Vector3 Vector3f; - -#endif // VECTOR3_H diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.cpp deleted file mode 100644 index 9802257a89..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.cpp +++ /dev/null @@ -1,249 +0,0 @@ -#include "Navigation.h" - -Navigation::Navigation(GPS *withGPS, Waypoints *withWP) : - _gps(withGPS), - _wp(withWP), - _hold_course(-1) -{ -} - -void -Navigation::update_gps() -{ - location.alt = _gps->altitude; - location.lng = _gps->longitude; - location.lat = _gps->latitude; - - // target_bearing is where we should be heading - bearing = get_bearing(&location, &next_wp); - - // waypoint distance from plane - distance = get_distance(&location, &next_wp); - - calc_bearing_error(); - calc_altitude_error(); - altitude_above_home = location.alt - home.alt; - - // check if we have missed the WP - _loiter_delta = (bearing - _old_bearing) / 100; - - // reset the old value - _old_bearing = bearing; - - // wrap values - if (_loiter_delta > 170) _loiter_delta -= 360; - if (_loiter_delta < -170) _loiter_delta += 360; - loiter_sum += abs(_loiter_delta); - - if (distance <= 0){ - distance = -1; - Serial.print("MSG wp error "); - Serial.println(distance, DEC); - } -} - -void -Navigation::load_first_wp(void) -{ - set_next_wp(_wp->get_waypoint_with_index(1)); -} - -void -Navigation::load_home(void) -{ - home = _wp->get_waypoint_with_index(0); -} - -void -Navigation::return_to_home_with_alt(uint32_t alt) -{ - Waypoints::WP loc = _wp->get_waypoint_with_index(0); - loc.alt += alt; - set_next_wp(loc); -} - -void -Navigation::reload_wp(void) -{ - set_next_wp(_wp->get_current_waypoint()); -} - -void -Navigation::load_wp_index(uint8_t i) -{ - _wp->set_index(i); - set_next_wp(_wp->get_current_waypoint()); -} - -void -Navigation::hold_location() -{ - // set_next_wp() XXX needs to be implemented -} - -void -Navigation::set_home(Waypoints::WP loc) -{ - _wp->set_waypoint_with_index(loc, 0); - home = loc; - //location = home; -} - -void -Navigation::set_next_wp(Waypoints::WP loc) -{ - prev_wp = next_wp; - next_wp = loc; - - if(_scaleLongDown == 0) - calc_long_scaling(loc.lat); - - total_distance = get_distance(&location, &next_wp); - distance = total_distance; - - bearing = get_bearing(&location, &next_wp); - _old_bearing = bearing; - - // clear loitering code - _loiter_delta = 0; - loiter_sum = 0; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); -} - -void -Navigation::calc_long_scaling(int32_t lat) -{ - // this is used to offset the shrinking longitude as we go towards the poles - float rads = (abs(lat) / T7) * 0.0174532925; - _scaleLongDown = cos(rads); - _scaleLongUp = 1.0f / cos(rads); -} - -void -Navigation::set_hold_course(int16_t b) -{ - if(b) - _hold_course = bearing; - else - _hold_course = -1; -} - -int16_t -Navigation::get_hold_course(void) -{ - return _hold_course; -} - -void -Navigation::calc_distance_error() -{ - //distance_estimate += (float)_gps->ground_speed * .0002 * cos(radians(bearing_error * .01)); - //distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_distance); - //distance = max(distance_estimate,10); -} - -void -Navigation::calc_bearing_error(void) -{ - if(_hold_course == -1){ - bearing_error = wrap_180(bearing - _gps->ground_course); - }else{ - bearing_error = _hold_course; - } -} - -int32_t -Navigation::wrap_180(int32_t error) -{ - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; -} - -int32_t -Navigation::wrap_360(int32_t angle) -{ - if (angle > 36000) angle -= 36000; - if (angle < 0) angle += 36000; - return angle; -} - -void -Navigation::set_bearing_error(int32_t error) -{ - bearing_error = wrap_180(error); -} - - -/***************************************** - * Altitude error with Airspeed correction - *****************************************/ -void -Navigation::calc_altitude_error(void) -{ - // limit climb rates - _target_altitude = next_wp.alt - ((float)((distance -20) * _offset_altitude) / (float)(total_distance - 20)); - if(prev_wp.alt > next_wp.alt){ - _target_altitude = constrain(_target_altitude, next_wp.alt, prev_wp.alt); - }else{ - _target_altitude = constrain(_target_altitude, prev_wp.alt, next_wp.alt); - } - altitude_error = _target_altitude - location.alt; -} - -void -Navigation::set_loiter_vector(int16_t v) -{ - // _vector = constrain(v, -18000, 18000); XXX needs to be implemented -} - -void -Navigation::update_crosstrack(void) -{ - // Crosstrack Error - // ---------------- - if (abs(bearing - _crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following - _crosstrack_error = sin(radians((bearing - _crosstrack_bearing) / 100)) * distance; // Meters we are off track line - bearing += constrain(_crosstrack_error * XTRACK_GAIN, -XTRACK_ENTRY_ANGLE, XTRACK_ENTRY_ANGLE); - } -} - -void -Navigation::reset_crosstrack(void) -{ - _crosstrack_bearing = get_bearing(&location, &next_wp); // Used for track following -} - -long -Navigation::get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2) -{ - if(loc1->lat == 0 || loc1->lng == 0) - return -1; - if(loc2->lat == 0 || loc2->lng == 0) - return -1; - if(_scaleLongDown == 0) - calc_long_scaling(loc2->lat); - float dlat = (float)(loc2->lat - loc1->lat); - float dlong = ((float)(loc2->lng - loc1->lng)) * _scaleLongDown; - return sqrt(sq(dlat) + sq(dlong)) * .01113195; -} - -long -Navigation::get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2) -{ - if(loc1->lat == 0 || loc1->lng == 0) - return -1; - if(loc2->lat == 0 || loc2->lng == 0) - return -1; - if(_scaleLongDown == 0) - calc_long_scaling(loc2->lat); - 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; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.h deleted file mode 100644 index 4fce19da63..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/Navigation.h +++ /dev/null @@ -1,76 +0,0 @@ - -#ifndef AP_NAVIGATION_h -#define AP_NAVIGATION_h - -#define XTRACK_GAIN 10 // Amount to compensate for crosstrack (degrees/100 per meter) -#define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100 -#include // ArduPilot GPS Library -#include "Waypoints.h" // ArduPilot Waypoints Library -#include "WProgram.h" - -#define T7 10000000 - -class Navigation { -public: - Navigation(GPS *withGPS, Waypoints *withWP); - - void update_gps(void); // called 50 Hz - void set_home(Waypoints::WP loc); - void set_next_wp(Waypoints::WP loc); - void load_first_wp(void); - void load_wp_with_index(uint8_t i); - void load_home(void); - void return_to_home_with_alt(uint32_t alt); - - void reload_wp(void); - void load_wp_index(uint8_t i); - void hold_location(); - void set_wp(Waypoints::WP loc); - - void set_hold_course(int16_t b); // -1 deisables, 0-36000 enables - int16_t get_hold_course(void); - - int32_t get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2); - int32_t get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2); - void set_bearing_error(int32_t error); - - void set_loiter_vector(int16_t v); - void update_crosstrack(void); - - int32_t wrap_180(int32_t error); // utility - int32_t wrap_360(int32_t angle); // utility - - int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate - int32_t distance; // meters : distance plane to next waypoint - int32_t altitude_above_home; // meters * 100 relative to home position - int32_t total_distance; // meters : distance between waypoints - int32_t bearing_error; // deg * 100 : 18000 to -18000 - int32_t altitude_error; // deg * 100 : 18000 to -18000 - - int16_t loiter_sum; - Waypoints::WP home, location, prev_wp, next_wp; - -private: - void calc_int32_t_scaling(int32_t lat); - void calc_bearing_error(void); - void calc_altitude_error(void); - void calc_distance_error(void); - void calc_long_scaling(int32_t lat); - void reset_crosstrack(void); - - int16_t _old_bearing; // used to track delta on the bearing - GPS *_gps; - Waypoints *_wp; - int32_t _crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target - float _crosstrack_error; // deg * 100 : 18000 to -18000 meters we are off trackline - int16_t _hold_course; // deg * 100 dir of plane - int32_t _target_altitude; // used for - int32_t _offset_altitude; // used for - float _altitude_estimate; - float _scaleLongUp; // used to reverse int32_ttitude scaling - float _scaleLongDown; // used to reverse int32_ttitude scaling - int16_t _loiter_delta; -}; - - -#endif // AP_NAVIGATION_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation/Navigation.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation/Navigation.pde deleted file mode 100644 index 19b0b54483..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation/Navigation.pde +++ /dev/null @@ -1,152 +0,0 @@ -/* - -This test assumes you are at the LOWl demo Airport - -*/ - -#include "Waypoints.h" -#include "Navigation.h" -#include "AP_GPS_IMU.h" -#include "AP_RC.h" - - -AP_GPS_IMU gps; -Navigation nav((GPS *) &gps); -AP_RC rc; - -#define CH_ROLL 0 -#define CH_PITCH 1 -#define CH_THROTTLE 2 -#define CH_RUDDER 3 - -#define CH3_MIN 1100 -#define CH3_MAX 1900 - -#define REVERSE_RUDDER 1 -#define REVERSE_ROLL 1 -#define REVERSE_PITCH 1 - -int8_t did_init_home; -int16_t servo_out[4]; -int16_t radio_trim[4] = {1500,1500,1100,1500}; -int16_t radio_in[4]; - -void setup() -{ - Serial.begin(38400); - Serial.println("Navigation test"); - Waypoints::WP location_B = {0, 0, 74260, 472586364, 113366012}; - nav.set_next_wp(location_B); - rc.init(radio_trim); -} - -void loop() -{ - delay(20); - gps.update(); - rc.read_pwm(); - for(int y=0; y<4; y++) { - //rc.set_ch_pwm(y, rc.input[y]); // send to Servos - radio_in[y] = rc.input[y]; - } - - servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45 * REVERSE_ROLL) / 500; - servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45 * REVERSE_PITCH) / 500; - servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45 * REVERSE_RUDDER) / 500; - servo_out[CH_THROTTLE] = (float)(radio_in[CH_THROTTLE] - CH3_MIN) / (float)(CH3_MAX - CH3_MIN) *100; - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); - - output_Xplane(); - if(gps.new_data /* && gps.fix */ ){ - if(did_init_home == 0){ - did_init_home = 1; - Waypoints::WP wp = {0, 0, gps.altitude, gps.lattitude, gps.longitude}; - nav.set_home(wp); - Serial.println("MSG init home"); - }else{ - nav.update_gps(); - } - //print_loc(); - } -} - -void print_loc() -{ - Serial.print("MSG GPS: "); - Serial.print(nav.location.lat, DEC); - Serial.print(", "); - Serial.print(nav.location.lng, DEC); - Serial.print(", "); - Serial.print(nav.location.alt, DEC); - Serial.print("\tDistance = "); - Serial.print(nav.distance, DEC); - Serial.print(" Bearing = "); - Serial.print(nav.bearing/100, DEC); - Serial.print(" Bearing err = "); - Serial.print(nav.bearing_error/100, DEC); - Serial.print(" alt err = "); - Serial.print(nav.altitude_error/100, DEC); - Serial.print(" Alt above home = "); - Serial.println(nav.altitude_above_home/100, DEC); -} - -void print_pwm() -{ - Serial.print("ch1 "); - Serial.print(rc.input[CH_ROLL],DEC); - Serial.print("\tch2: "); - Serial.print(rc.input[CH_PITCH],DEC); - Serial.print("\tch3 :"); - Serial.print(rc.input[CH_THROTTLE],DEC); - Serial.print("\tch4 :"); - Serial.println(rc.input[CH_RUDDER],DEC); -} - - - - -byte buf_len = 0; -byte out_buffer[32]; - -void output_Xplane(void) -{ - // output real-time sensor data - Serial.print("AAA"); // Message preamble - output_int((int)(servo_out[CH_ROLL]*100)); // 0 bytes 0,1 - output_int((int)(servo_out[CH_PITCH]*100)); // 1 bytes 2,3 - output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4,5 - output_int((int)(servo_out[CH_RUDDER]*100)); // 3 bytes 6,7 - output_int((int)nav.distance); // 4 bytes 8,9 - output_int((int)nav.bearing_error); // 5 bytes 10,11 - output_int((int)nav.altitude_error); // 6 bytes 12,13 - output_int(0); // 7 bytes 14,15 - output_byte(1); // 8 bytes 16 - output_byte(1); // 9 bytes 17 - - // print out the buffer and checksum - // --------------------------------- - print_buffer(); -} - -void output_int(int value) -{ - //buf_len += 2; - out_buffer[buf_len++] = value & 0xff; - out_buffer[buf_len++] = (value >> 8) & 0xff; -} -void output_byte(byte value) -{ - out_buffer[buf_len++] = value; -} - -void print_buffer() -{ - byte ck_a = 0; - byte ck_b = 0; - for (byte i = 0;i < buf_len; i++){ - Serial.print (out_buffer[i]); - } - Serial.print('\r'); - Serial.print('\n'); - buf_len = 0; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde deleted file mode 100644 index 5c0f21739c..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include -#include - - -AP_GPS_IMU gps; -Navigation nav((GPS *) & gps); - -void setup() -{ - Serial.begin(38400); - Serial.println("Navigation test"); - - Waypoints::WP location_A = {0, 0, 38.579633 * T7, -122.566265 * T7, 10000}; - Waypoints::WP location_B = {0, 0, 38.578743 * T7, -122.572782 * T7, 5000}; - - long distance = nav.get_distance(&location_A, &location_B); - long bearing = nav.get_bearing(&location_A, &location_B); - - Serial.print("\tDistance = "); - Serial.print(distance, DEC); - Serial.print(" Bearing = "); - Serial.println(bearing, DEC); -} - -void loop() -{ -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.cpp deleted file mode 100644 index 90655df9f8..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.cpp +++ /dev/null @@ -1,472 +0,0 @@ -#include "DCM.h" - -// XXX HACKS -APM_ADC adc; - -// XXX END HACKS - - -#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer -#define ADC_CONSTRAINT 900 - -#define Kp_ROLLPITCH 0.0014 //0.015 // Pitch&Roll Proportional Gain -#define Ki_ROLLPITCH 0.0000003 // 0.00001 Pitch&Roll Integrator Gain -#define Kp_YAW 1.2 // 1.2 Yaw Porportional Gain -#define Ki_YAW 0.00005 // 0.00005 Yaw Integrator Gain - -// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ -const uint8_t AP_DCM::_sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware -const int AP_DCM::_sensor_signs[] = {1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1} !!!! These are probably not right - -// Temp compensation curve constants -// These must be produced by measuring data and curve fitting -// [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants] -const float AP_DCM::_gyro_temp_curve[3][3] = { - {1665,0,0}, - {1665,0,0}, - {1665,0,0} -}; // values may migrate to a Config file - - - -// Constructors //////////////////////////////////////////////////////////////// -AP_DCM::AP_DCM(APM_Compass *withCompass) : - _compass(withCompass), - _dcm_matrix(1, 0, 0, - 0, 1, 0, - 0, 0, 1), - _G_Dt(0.02), - _course_over_ground_x(0), - _course_over_ground_y(1) -{ -} - -void -AP_DCM::update_DCM(void) -{ - read_adc_raw(); // Get current values for IMU sensors - matrix_update(); // Integrate the DCM matrix - normalize(); // Normalize the DCM matrix - drift_correction(); // Perform drift correction - euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation -} - - -// Read the 6 ADC channels needed for the IMU -// ------------------------------------------ -void -AP_DCM::read_adc_raw(void) -{ - int tc_temp = adc.Ch(_gyro_temp_ch); - for (int i = 0; i < 6; i++) { - _adc_in[i] = adc.Ch(_sensors[i]); - if (i < 3) { // XXX magic numbers! - _adc_in[i] -= _gyro_temp_comp(i, tc_temp); // Subtract temp compensated typical gyro bias - } else { - _adc_in[i] -= 2025; // Subtract typical accel bias - } - } -} - -// Returns the temperature compensated raw gyro value -//--------------------------------------------------- -float -AP_DCM::_gyro_temp_comp(int i, int temp) const -{ - // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2 - //------------------------------------------------------------------------ - return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp; -} - -// Returns an analog value with the offset removed -// ----------------- -float -AP_DCM::read_adc(int select) -{ - float temp; - if (_sensor_signs[select] < 0) - temp = (_adc_offset[select] - _adc_in[select]); - else - temp = (_adc_in[select] - _adc_offset[select]); - - if (abs(temp) > ADC_CONSTRAINT) - adc_constraints++; // We keep track of the number of times we constrain the ADC output for performance reporting - -/* -// For checking the pitch/roll drift correction gain time constants -switch (select) { - case 3: - return 0; - break; - case 4: - return 0; - break; - case 5: - return 400; - break; -} -*/ - - -//End of drift correction gain test code - - return constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values -} - -/**************************************************/ -void -AP_DCM::normalize(void) -{ - float error = 0; - DCM_Vector temporary[3]; - - uint8_t problem = 0; - - error = -_dcm_matrix(0).dot_product(_dcm_matrix(1)) * 0.5; // eq.19 - - temporary[0] = _dcm_matrix(1) * error + _dcm_matrix(0); // eq.19 - temporary[1] = _dcm_matrix(0) * error + _dcm_matrix(1); // eq.19 - - temporary[2] = temporary[0] ^ temporary[1]; // c= a x b // eq.20 - - _dcm_matrix(0) = _renorm(temporary[0], problem); - _dcm_matrix(1) = _renorm(temporary[1], problem); - _dcm_matrix(2) = _renorm(temporary[2], problem); - - if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! - _dcm_matrix(0, 0)= 1.0f; - _dcm_matrix(0, 1)= 0.0f; - _dcm_matrix(0, 2)= 0.0f; - _dcm_matrix(1, 0)= 0.0f; - _dcm_matrix(1, 1)= 1.0f; - _dcm_matrix(1, 2)= 0.0f; - _dcm_matrix(2, 0)= 0.0f; - _dcm_matrix(2, 1)= 0.0f; - _dcm_matrix(2, 2)= 1.0f; - } -} - -DCM_Vector -AP_DCM::_renorm(DCM_Vector const &a, uint8_t &problem) -{ - float renorm; - - renorm = a.dot_product(a); - - if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with Taylor expansion - renorm = 0.5 * (3 - renorm); // eq.21 - } else if (renorm < 100.0f && renorm > 0.01f) { - renorm = 1.0 / sqrt(renorm); - renorm_sqrt_count++; - } else { - problem = 1; - renorm_blowup_count++; - } - - return(a * renorm); -} - -/**************************************************/ -void -AP_DCM::drift_correction(void) -{ - //Compensation the Roll, Pitch and Yaw drift. - float mag_heading_x; - float mag_heading_y; - float error_course = 0; - static float scaled_omega_P[3]; - static float scaled_omega_I[3]; - float accel_magnitude; - float accel_weight; - float integrator_magnitude; - - //*****Roll and Pitch*************** - - // Calculate the magnitude of the accelerometer vector - accel_magnitude = _accel_vector.magnitude() / GRAVITY; // Scale to gravity. - - // Dynamic weighting of accelerometer info (reliability filter) - // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - accel_weight = constrain(1 - 2 * abs(1 - accel_magnitude), 0, 1); // - - // We monitor the amount that the accelerometer based drift correction is deweighted for performanc reporting - imu_health = imu_health + 0.02 * (accel_weight-.5); - imu_health = constrain(imu_health, 0, 1); - - // adjust the ground of reference - _error_roll_pitch = _accel_vector ^ _dcm_matrix(2); - - // error_roll_pitch are in Accel ADC units - // Limit max error_roll_pitch to limit max omega_P and omega_I - _error_roll_pitch(0) = constrain(_error_roll_pitch(0), -50, 50); - _error_roll_pitch(1) = constrain(_error_roll_pitch(1), -50, 50); - _error_roll_pitch(2) = constrain(_error_roll_pitch(2), -50, 50); - - _omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight); - _omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight); - - //*****YAW*************** - - if (_compass) { - // We make the gyro YAW drift correction based on compass magnetic heading - error_course= (_dcm_matrix(0, 0) * _compass->Heading_Y) - (_dcm_matrix(1, 0) * _compass->Heading_X); // Calculating YAW error - } else { - // Use GPS Ground course to correct yaw gyro drift - if (ground_speed >= SPEEDFILT) { - // Optimization: We have precalculated course_over_ground_x and course_over_ground_y (Course over Ground X and Y) from GPS info - error_course = (_dcm_matrix(0, 0) * _course_over_ground_y) - (_dcm_matrix(1, 0) * _course_over_ground_x); // Calculating YAW error - } - } - _error_yaw = _dcm_matrix(2) * error_course; // Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - - _omega_P += _error_yaw * Kp_YAW; // Adding Proportional. - _omega_I += _error_yaw * Ki_YAW; // adding integrator to the omega_I - - // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros - integrator_magnitude = sqrt(_omega_I.dot_product(_omega_I)); - if (integrator_magnitude > radians(300)) { - _omega_I *= (0.5f * radians(300) / integrator_magnitude); - } - -} - -/**************************************************/ -void -AP_DCM::_accel_adjust(void) -{ - _accel_vector(1) += accel_scale((ground_speed / 100) * _omega(2)); // Centrifugal force on Acc_y = GPS_speed * GyroZ - _accel_vector(2) -= accel_scale((ground_speed / 100) * _omega(1)); // Centrifugal force on Acc_z = GPS_speed * GyroY -} - - -/**************************************************/ -void -AP_DCM::matrix_update(void) -{ - DCM_Matrix update_matrix; - - _gyro_vector(0) = gyro_scaled_X(read_adc(0)); // gyro x roll - _gyro_vector(1) = gyro_scaled_Y(read_adc(1)); // gyro y pitch - _gyro_vector(2) = gyro_scaled_Z(read_adc(2)); // gyro Z yaw - - //Record when you saturate any of the gyros. - if((abs(_gyro_vector(0)) >= radians(300)) || - (abs(_gyro_vector(1)) >= radians(300)) || - (abs(_gyro_vector(2)) >= radians(300))) - gyro_sat_count++; - -/* -Serial.print (__adc_in[0]); -Serial.print (" "); -Serial.print (_adc_offset[0]); -Serial.print (" "); -Serial.print (_gyro_vector(0)); -Serial.print (" "); -Serial.print (__adc_in[1]); -Serial.print (" "); -Serial.print (_adc_offset[1]); -Serial.print (" "); -Serial.print (_gyro_vector(1)); -Serial.print (" "); -Serial.print (__adc_in[2]); -Serial.print (" "); -Serial.print (_adc_offset[2]); -Serial.print (" "); -Serial.println (_gyro_vector(2)); -*/ - -// _accel_vector(0) = read_adc(3); // acc x -// _accel_vector(1) = read_adc(4); // acc y -// _accel_vector(2) = read_adc(5); // acc z - // Low pass filter on accelerometer data (to filter vibrations) - _accel_vector(0) = _accel_vector(0) * 0.6 + (float)read_adc(3) * 0.4; // acc x - _accel_vector(1) = _accel_vector(1) * 0.6 + (float)read_adc(4) * 0.4; // acc y - _accel_vector(2) = _accel_vector(2) * 0.6 + (float)read_adc(5) * 0.4; // acc z - - _omega = _gyro_vector + _omega_I; // adding proportional term - _omega_vector = _omega + _omega_P; // adding Integrator term - - _accel_adjust(); // Remove centrifugal acceleration. - - #if OUTPUTMODE == 1 - update_matrix(0, 0) = 0; - update_matrix(0, 1) = -_G_Dt * _omega_vector(2); // -z - update_matrix(0, 2) = _G_Dt * _omega_vector(1); // y - update_matrix(1, 0) = _G_Dt * _omega_vector(2); // z - update_matrix(1, 1) = 0; - update_matrix(1, 2) = -_G_Dt * _omega_vector(0); // -x - update_matrix(2, 0) = -_G_Dt * _omega_vector(1); // -y - update_matrix(2, 1) = _G_Dt * _omega_vector(0); // x - update_matrix(2, 2) = 0; - #else // Uncorrected data (no drift correction) - update_matrix(0, 0) = 0; - update_matrix(0, 1) = -_G_Dt * _gyro_vector(2); // -z - update_matrix(0, 2) = _G_Dt * _gyro_vector(1); // y - update_matrix(1, 0) = _G_Dt * _gyro_vector(2); // z - update_matrix(1, 1) = 0; - update_matrix(1, 2) = -_G_Dt * _gyro_vector(0); - update_matrix(2, 0) = -_G_Dt * _gyro_vector(1); - update_matrix(2, 1) = _G_Dt * _gyro_vector(0); - update_matrix(2, 2) = 0; - #endif - - // update - _dcm_matrix += _dcm_matrix * update_matrix; - -/* -Serial.print (_G_Dt * 1000); -Serial.print (" "); -Serial.print (dcm_matrix(0, 0)); -Serial.print (" "); -Serial.print (dcm_matrix(0, 1)); -Serial.print (" "); -Serial.print (dcm_matrix(0, 2)); -Serial.print (" "); -Serial.print (dcm_matrix(1, 0)); -Serial.print (" "); -Serial.print (dcm_matrix(1, 1)); -Serial.print (" "); -Serial.print (dcm_matrix(1, 2)); -Serial.print (" "); -Serial.print (dcm_matrix(2, 0)); -Serial.print (" "); -Serial.print (dcm_matrix(2, 1)); -Serial.print (" "); -Serial.println (dcm_matrix(2, 2)); -*/ -} - -/**************************************************/ -void -AP_DCM::euler_angles(void) -{ - #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes) - roll = atan2(_accel_vector(1), _accel_vector(2)); // atan2(acc_y, acc_z) - roll_sensor = degrees(roll) * 100; - pitch = -asin((_accel_vector(0)) / (double)GRAVITY); // asin(acc_x) - pitch_sensor = degrees(pitch) * 100; - yaw = 0; - #else - pitch = -asin(_dcm_matrix(2, 0)); - pitch_sensor = degrees(pitch) * 100; - roll = atan2(_dcm_matrix(2, 1), _dcm_matrix(2, 2)); - roll_sensor = degrees(roll) * 100; - yaw = atan2(_dcm_matrix(1, 0), _dcm_matrix(0, 0)); - yaw_sensor = degrees(yaw) * 100; - #endif - - /* - Serial.print ("Roll "); - Serial.print (roll_sensor / 100); - Serial.print (", Pitch "); - Serial.print (pitch_sensor / 100); - Serial.print (", Yaw "); - Serial.println (yaw_sensor / 100); - */ -} - -/**************************************************/ -//Computes the dot product of two vectors -float -DCM_Vector::dot_product(DCM_Vector const &vector2) const -{ - float op = 0; - - for(int c = 0; c < 3; c++) - op += _v[c] * vector2(c); - - return op; -} - -// cross-product -DCM_Vector -DCM_Vector::operator^(DCM_Vector const &a) const -{ - DCM_Vector result; - - result(0) = (_v[1] * a(2)) - (_v[2] * a(1)); - result(1) = (_v[2] * a(0)) - (_v[0] * a(2)); - result(2) = (_v[0] * a(1)) - (_v[1] * a(0)); - - return(result); -} - -// scale -DCM_Vector -DCM_Vector::operator*(float scale) const -{ - DCM_Vector result; - - result(0) = _v[0] * scale; - result(1) = _v[1] * scale; - result(2) = _v[2] * scale; - - return(result); -} - -// scale -void -DCM_Vector::operator*=(float scale) -{ - _v[0] *= scale; - _v[1] *= scale; - _v[2] *= scale; -} - -// add -DCM_Vector -DCM_Vector::operator+(DCM_Vector const &a) const -{ - DCM_Vector result; - - result(0) = _v[0] + a(0); - result(1) = _v[1] + a(1); - result(2) = _v[2] + a(2); - - return(result); -} - -// add -void -DCM_Vector::operator+=(DCM_Vector const &a) -{ - _v[0] += a(0); - _v[1] += a(1); - _v[2] += a(2); -} - -// magnitude -float -DCM_Vector::magnitude(void) const -{ - return(sqrt((_v[0] * _v[0]) + - (_v[1] * _v[1]) + - (_v[2] * _v[2]))); -} - -// 3x3 matrix multiply -DCM_Matrix -DCM_Matrix::operator*(DCM_Matrix const &a) const -{ - DCM_Matrix result; - - for (int x = 0; x < 3; x++) { - for (int y = 0; y < 3; y++) { - result(x, y) = - _m[x](0) * a(0, y) + - _m[x](1) * a(1, y) + - _m[x](2) * a(2, y); - } - } - return(result); -} - -// 3x3 matrix add -void -DCM_Matrix::operator+=(DCM_Matrix const &a) -{ - for (int x = 0; x < 3; x++) - for (int y = 0; y < 3; y++) - _m[x](y) += a(x,y); -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.h deleted file mode 100644 index 76dbb71381..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/DCM.h +++ /dev/null @@ -1,147 +0,0 @@ -#ifndef AP_DCM_h -#define AP_DCM_h - -#include -//#include "WProgram.h" - -//////////////////////////////////////////////////////////////////////////////// -// XXX HACKS -class APM_Compass { -public: - int Heading_X; - int Heading_Y; -}; - -typedef uint8_t byte; - -class APM_ADC { -public: - int Ch(int c) {return ~c;}; -}; - -extern int ground_speed; -extern int pitch; -extern int yaw; -extern int roll; -extern int roll_sensor; -extern int pitch_sensor; -extern int yaw_sensor; -#define SPEEDFILT 100 - -// XXX warning, many of these are nonsense just to make the compiler think -#define abs(_x) (((_x) < 0) ? -(_x) : (_x)) -#define constrain(_x, _min, _max) (((_x) < (_min)) ? (_min) : (((_x) > (_max)) ? (_max) : (_x))) -#define sqrt(_x) ((_x) / 2) // !!! -#define radians(_x) ((_x) / (180 * 3.14)) // !!! shoot me... -#define degrees(_x) ((_x) * (180 / 3.14)) -#define accel_scale(_x) ((_x) * 3) // !!! -#define gyro_scaled_X(_x) ((_x) / 3) -#define gyro_scaled_Y(_x) ((_x) / 3) -#define gyro_scaled_Z(_x) ((_x) / 3) -#define asin(_x) ((_x) * 5) -#define atan2(_x, _y) (((_x) + (_y)) / 5) - -// XXX END HACKS -//////////////////////////////////////////////////////////////////////////////// - -class DCM_Vector { -public: - DCM_Vector(float v0 = 0, float v1 = 0, float v2 = 0); - - // access vector elements with obj(element) - float& operator() (int x) {return _v[x];}; - float operator() (int x) const {return _v[x];}; - - DCM_Vector operator+ (DCM_Vector const &a) const; // add - void operator+= (DCM_Vector const &a); // add - DCM_Vector operator^ (DCM_Vector const &a) const; // cross-product - DCM_Vector operator* (float scale) const; // scale - void operator*= (float scale); // scale - - float dot_product(DCM_Vector const &v2) const; - float magnitude(void) const; - -private: - float _v[3]; -}; - -class DCM_Matrix { -public: - DCM_Matrix(float m00 = 0, float m01 = 0, float m02 = 0, - float m10 = 0, float m11 = 0, float m12 = 0, - float m20 = 0, float m21 = 0, float m22 = 0); - - // access matrix elements with obj(x,y) - float& operator() (int x, int y) {return _m[x](y);}; - float operator() (int x, int y) const {return _m[x](y);}; - - // access matrix columns with obj(x) - DCM_Vector& operator() (int x) {return _m[x];}; - DCM_Vector operator() (int x) const {return _m[x];}; - - // matrix multiply - DCM_Matrix operator* (DCM_Matrix const &a) const; - - // matrix add - void operator+= (DCM_Matrix const &a); - -private: - DCM_Vector _m[3]; -}; - - -class AP_DCM -{ -public: - // Methods - AP_DCM(APM_Compass *withCompass); - void update_DCM(void); //G_Dt - - // XXX these are all private (called by update_DCM only?) - void read_adc_raw(void); - void euler_angles(void); - void matrix_update(void); - void drift_correction(void); - void normalize(void); - float read_adc(int select); - - float imu_health; //Metric based on accel gain deweighting - byte gyro_sat_count; - byte adc_constraints; - byte renorm_sqrt_count; - byte renorm_blowup_count; - -private: - // Methods - void _accel_adjust(void); - float _gyro_temp_comp(int i, int temp) const; - DCM_Vector _renorm(DCM_Vector const &a, uint8_t &problem); - - // members - APM_Compass *_compass; - - DCM_Matrix _dcm_matrix; - - float _adc_in[6]; // array that store the 6 ADC channels used by IMU - float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers - float _G_Dt; // Integration time for the gyros (DCM algorithm) - DCM_Vector _accel_vector; // Store the acceleration in a vector - DCM_Vector _gyro_vector; //Store the gyros turn rate in a vector - DCM_Vector _omega_vector; //Corrected Gyro_Vector data - DCM_Vector _omega_P; //Omega Proportional correction - DCM_Vector _omega_I; //Omega Integrator - DCM_Vector _omega; - DCM_Vector _error_roll_pitch; - DCM_Vector _error_yaw; - float _errorCourse; - float _course_over_ground_x; //Course overground X axis - float _course_over_ground_y; //Course overground Y axis - - // constants - static const uint8_t _sensors[6]; - static const int _sensor_signs[9]; - static const uint8_t _gyro_temp_ch = 3; // The ADC channel reading the gyro temperature - static const float _gyro_temp_curve[3][3]; -}; - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/examples/DCM_test/DCM_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/examples/DCM_test/DCM_test.pde deleted file mode 100644 index c7225d44ae..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DCM/examples/DCM_test/DCM_test.pde +++ /dev/null @@ -1,38 +0,0 @@ -/* - Example of APM_Compass library (HMC5843 sensor). - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com -*/ - -//#include -#include // Compass Library - -unsigned long timer; - -void setup() -{ - DCM.init(); // Initialization - Serial.begin(38400); - Serial.println("Compass library test (HMC5843)"); - delay(1000); - timer = millis(); -} - -void loop() -{ - float tmp; - - if((millis()- timer) > 100){ - timer = millis(); - APM_Compass.Read(); - APM_Compass.Calculate(0, 0); // roll = 0, pitch = 0 for this example - Serial.print("Heading:"); - Serial.print(ToDeg(APM_Compass.Heading)); - Serial.print(" ("); - Serial.print(APM_Compass.Mag_X); - Serial.print(","); - Serial.print(APM_Compass.Mag_Y); - Serial.print(","); - Serial.print(APM_Compass.Mag_Z); - Serial.println(" )"); - } -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.cpp deleted file mode 100644 index d7c4bba837..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.cpp +++ /dev/null @@ -1,342 +0,0 @@ -/* - DataFlash.cpp - DataFlash log library for AT45DB161 - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - This code works with boards based on ATMega168/328 and ATMega1280 using SPI port - - This library 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. - - Dataflash library for AT45DB161D flash memory - Memory organization : 4096 pages of 512 bytes - - Maximun write bandwidth : 512 bytes in 14ms - This code is written so the master never has to wait to write the data on the eeprom - - Methods: - Init() : Library initialization (SPI initialization) - StartWrite(page) : Start a write session. page=start page. - WriteByte(data) : Write a byte - WriteInt(data) : Write an integer (2 bytes) - WriteLong(data) : Write a long (4 bytes) - StartRead(page) : Start a read on (page) - GetWritePage() : Returns the last page written to - GetPage() : Returns the last page read - ReadByte() - ReadInt() - ReadLong() - - Properties: - -*/ - -extern "C" { - // AVR LibC Includes - #include - #include - #include "WConstants.h" -} - -#include "DataFlash.h" - -#define OVERWRITE_DATA 0 // 0: When reach the end page stop, 1: Start overwritten from page 1 - -// *** INTERNAL FUNCTIONS *** -unsigned char dataflash_SPI_transfer(unsigned char output) -{ - SPDR = output; // Start the transmission - while (!(SPSR & (1<> 6)); - dataflash_SPI_transfer((unsigned char)(PageAdr << 2)); - dataflash_SPI_transfer(0x00); // don´t care bytes - - dataflash_CS_inactive(); //initiate the transfer - dataflash_CS_active(); - - while(!ReadStatus()); //monitor the status register, wait until busy-flag is high -} - -void DataFlash_Class::BufferToPage (unsigned char BufferNum, unsigned int PageAdr, unsigned char wait) -{ - dataflash_CS_inactive(); // Reset dataflash command decoder - dataflash_CS_active(); - - if (BufferNum==1) - dataflash_SPI_transfer(DF_BUFFER_1_TO_PAGE_WITH_ERASE); - else - dataflash_SPI_transfer(DF_BUFFER_2_TO_PAGE_WITH_ERASE); - dataflash_SPI_transfer((unsigned char)(PageAdr >> 6)); - dataflash_SPI_transfer((unsigned char)(PageAdr << 2)); - dataflash_SPI_transfer(0x00); // don´t care bytes - - dataflash_CS_inactive(); //initiate the transfer - dataflash_CS_active(); - - // Check if we need to wait to write the buffer to memory or we can continue... - if (wait) - while(!ReadStatus()); //monitor the status register, wait until busy-flag is high -} - -void DataFlash_Class::BufferWrite (unsigned char BufferNum, unsigned int IntPageAdr, unsigned char Data) -{ - dataflash_CS_inactive(); // Reset dataflash command decoder - dataflash_CS_active(); - - if (BufferNum==1) - dataflash_SPI_transfer(DF_BUFFER_1_WRITE); - else - dataflash_SPI_transfer(DF_BUFFER_2_WRITE); - dataflash_SPI_transfer(0x00); //don't cares - dataflash_SPI_transfer((unsigned char)(IntPageAdr>>8)); //upper part of internal buffer address - dataflash_SPI_transfer((unsigned char)(IntPageAdr)); //lower part of internal buffer address - dataflash_SPI_transfer(Data); //write data byte -} - -unsigned char DataFlash_Class::BufferRead (unsigned char BufferNum, unsigned int IntPageAdr) -{ - byte tmp; - - dataflash_CS_inactive(); // Reset dataflash command decoder - dataflash_CS_active(); - - if (BufferNum==1) - dataflash_SPI_transfer(DF_BUFFER_1_READ); - else - dataflash_SPI_transfer(DF_BUFFER_2_READ); - dataflash_SPI_transfer(0x00); //don't cares - dataflash_SPI_transfer((unsigned char)(IntPageAdr>>8)); //upper part of internal buffer address - dataflash_SPI_transfer((unsigned char)(IntPageAdr)); //lower part of internal buffer address - dataflash_SPI_transfer(0x00); //don't cares - tmp = dataflash_SPI_transfer(0x00); //read data byte - - return (tmp); -} -// *** END OF INTERNAL FUNCTIONS *** - -void DataFlash_Class::PageErase (unsigned int PageAdr) -{ - dataflash_CS_inactive(); //make sure to toggle CS signal in order - dataflash_CS_active(); //to reset Dataflash command decoder - dataflash_SPI_transfer(DF_PAGE_ERASE); // Command - dataflash_SPI_transfer((unsigned char)(PageAdr >> 6)); //upper part of page address - dataflash_SPI_transfer((unsigned char)(PageAdr << 2)); //lower part of page address and MSB of int.page adr. - dataflash_SPI_transfer(0x00); // "dont cares" - dataflash_CS_inactive(); //initiate flash page erase - dataflash_CS_active(); - while(!ReadStatus()); -} - -// *** DATAFLASH PUBLIC FUNCTIONS *** -void DataFlash_Class::StartWrite(int PageAdr) -{ - df_BufferNum=1; - df_BufferIdx=0; - df_PageAdr=PageAdr; - df_Stop_Write=0; - WaitReady(); -} - -void DataFlash_Class::WriteByte(byte data) -{ - if (!df_Stop_Write) - { - BufferWrite(df_BufferNum,df_BufferIdx,data); - df_BufferIdx++; - if (df_BufferIdx >= 512) // End of buffer? - { - df_BufferIdx=0; - BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT - df_PageAdr++; - if (OVERWRITE_DATA==1) - { - if (df_PageAdr>=4096) // If we reach the end of the memory, start from the begining - df_PageAdr = 1; - } - else - { - if (df_PageAdr>=4096) // If we reach the end of the memory, stop here - df_Stop_Write=1; - } - - if (df_BufferNum==1) // Change buffer to continue writing... - df_BufferNum=2; - else - df_BufferNum=1; - } - } -} - -void DataFlash_Class::WriteInt(int data) -{ - WriteByte(data>>8); // High byte - WriteByte(data&0xFF); // Low byte -} - -void DataFlash_Class::WriteLong(long data) -{ - WriteByte(data>>24); // First byte - WriteByte(data>>16); - WriteByte(data>>8); - WriteByte(data&0xFF); // Last byte -} - -// Get the last page written to -int DataFlash_Class::GetWritePage() -{ - return(df_PageAdr); -} - -// Get the last page read -int DataFlash_Class::GetPage() -{ - return(df_Read_PageAdr-1); -} - -void DataFlash_Class::StartRead(int PageAdr) -{ - df_Read_BufferNum=1; - df_Read_BufferIdx=0; - df_Read_PageAdr=PageAdr; - WaitReady(); - PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write Memory page to buffer - df_Read_PageAdr++; -} - -byte DataFlash_Class::ReadByte() -{ - byte result; - - WaitReady(); - result = BufferRead(df_Read_BufferNum,df_Read_BufferIdx); - df_Read_BufferIdx++; - if (df_Read_BufferIdx >= 512) // End of buffer? - { - df_Read_BufferIdx=0; - PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write memory page to Buffer - df_Read_PageAdr++; - if (df_Read_PageAdr>=4096) // If we reach the end of the memory, start from the begining - { - df_Read_PageAdr = 0; - df_Read_END = true; - } - } - return result; -} - -int DataFlash_Class::ReadInt() -{ - int result; - - result = ReadByte(); // High byte - result = (result<<8) | ReadByte(); // Low byte - return result; -} - -long DataFlash_Class::ReadLong() -{ - long result; - - result = ReadByte(); // First byte - result = (result<<8) | ReadByte(); - result = (result<<8) | ReadByte(); - result = (result<<8) | ReadByte(); // Last byte - return result; -} - -// make one instance for the user to use -DataFlash_Class DataFlash; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.h deleted file mode 100644 index 037421c989..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/DataFlash.h +++ /dev/null @@ -1,86 +0,0 @@ -/* ************************************************************ */ -/* Test for DataFlash Log library */ -/* ************************************************************ */ -#ifndef DataFlash_h -#define DataFlash_h - -// arduino mega SPI pins -#if defined(__AVR_ATmega1280__) - #define DF_DATAOUT 51 // MOSI - #define DF_DATAIN 50 // MISO - #define DF_SPICLOCK 52 // SCK - #define DF_SLAVESELECT 53 // SS (PB0) - #define DF_RESET 31 // RESET (PC6) -#else // normal arduino SPI pins... - #define DF_DATAOUT 11 //MOSI - #define DF_DATAIN 12 //MISO - #define DF_SPICLOCK 13 //SCK - #define DF_SLAVESELECT 10 //SS -#endif - -// AT45DB161D Commands (from Datasheet) -#define DF_TRANSFER_PAGE_TO_BUFFER_1 0x53 -#define DF_TRANSFER_PAGE_TO_BUFFER_2 0x55 -#define DF_STATUS_REGISTER_READ 0xD7 -#define DF_READ_MANUFACTURER_AND_DEVICE_ID 0x9F -#define DF_PAGE_READ 0xD2 -#define DF_BUFFER_1_READ 0xD4 -#define DF_BUFFER_2_READ 0xD6 -#define DF_BUFFER_1_WRITE 0x84 -#define DF_BUFFER_2_WRITE 0x87 -#define DF_BUFFER_1_TO_PAGE_WITH_ERASE 0x83 -#define DF_BUFFER_2_TO_PAGE_WITH_ERASE 0x86 -#define DF_PAGE_ERASE 0x81 -#define DF_BLOCK_ERASE 0x50 -#define DF_SECTOR_ERASE 0x7C -#define DF_CHIP_ERASE_0 0xC7 -#define DF_CHIP_ERASE_1 0x94 -#define DF_CHIP_ERASE_2 0x80 -#define DF_CHIP_ERASE_3 0x9A - -class DataFlash_Class -{ - private: - // DataFlash Log variables... - unsigned char df_BufferNum; - unsigned char df_Read_BufferNum; - unsigned int df_BufferIdx; - unsigned int df_Read_BufferIdx; - unsigned int df_PageAdr; - unsigned int df_Read_PageAdr; - unsigned char df_Read_END; - unsigned char df_Stop_Write; - //Methods - unsigned char BufferRead (unsigned char BufferNum, unsigned int IntPageAdr); - void BufferWrite (unsigned char BufferNum, unsigned int IntPageAdr, unsigned char Data); - void BufferToPage (unsigned char BufferNum, unsigned int PageAdr, unsigned char wait); - void PageToBuffer(unsigned char BufferNum, unsigned int PageAdr); - void WaitReady(); - unsigned char ReadStatus(); - - public: - unsigned char df_manufacturer; - unsigned char df_device_0; - unsigned char df_device_1; - - DataFlash_Class(); // Constructor - void Init(); - void ReadManufacturerID(); - int GetPage(); - int GetWritePage(); - void PageErase (unsigned int PageAdr); - // Write methods - void StartWrite(int PageAdr); - void WriteByte(unsigned char data); - void WriteInt(int data); - void WriteLong(long data); - // Read methods - void StartRead(int PageAdr); - unsigned char ReadByte(); - int ReadInt(); - long ReadLong(); -}; - -extern DataFlash_Class DataFlash; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde deleted file mode 100644 index 8ffc712d72..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde +++ /dev/null @@ -1,99 +0,0 @@ -/* - Example of DataFlash library. - Code by Jordi Muñoz and Jose Julio. DIYDrones.com -*/ - -#include - -#define HEAD_BYTE1 0xA3 -#define HEAD_BYTE2 0x95 - -void setup() -{ - Serial.begin(57600); - DataFlash.Init(); // DataFlash initialization - - Serial.println("Dataflash Log Test 1.0"); - - // Test - delay(20); - DataFlash.ReadManufacturerID(); - delay(10); - Serial.print("Manufacturer:"); - Serial.print(int(DataFlash.df_manufacturer)); - Serial.print(","); - Serial.print(int(DataFlash.df_device_0)); - Serial.print(","); - Serial.print(int(DataFlash.df_device_1)); - Serial.println(); - - // We start to write some info (sequentialy) starting from page 1 - // This is similar to what we will do... - DataFlash.StartWrite(1); - Serial.println("Writing to flash... wait..."); - for (int i=0;i<1000;i++) // Write 1000 packets... - { - // We write packets of binary data... (without worry about nothing more) - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteInt(2000+i); - DataFlash.WriteInt(2001+i); - DataFlash.WriteInt(2002+i); - DataFlash.WriteInt(2003+i); - DataFlash.WriteLong((long)i*5000); - DataFlash.WriteLong((long)i*16268); - DataFlash.WriteByte(0xA2); // 2 bytes of checksum (example) - DataFlash.WriteByte(0x4E); - delay(10); - } - delay(100); -} - -void loop() -{ - int i; - byte tmp_byte1; - byte tmp_byte2; - int tmp_int; - long tmp_long; - - Serial.println("Start reading page 1..."); - DataFlash.StartRead(1); // We start reading from page 1 - for (i=0;i<200;i++) // Read 200 packets... - { - tmp_byte1=DataFlash.ReadByte(); - tmp_byte2=DataFlash.ReadByte(); - Serial.print("PACKET:"); - if ((tmp_byte1==HEAD_BYTE1)&&(tmp_byte1==HEAD_BYTE1)) - { - // Read 4 ints... - tmp_int=DataFlash.ReadInt(); - Serial.print(tmp_int); - Serial.print(","); - tmp_int=DataFlash.ReadInt(); - Serial.print(tmp_int); - Serial.print(","); - tmp_int=DataFlash.ReadInt(); - Serial.print(tmp_int); - Serial.print(","); - tmp_int=DataFlash.ReadInt(); - Serial.print(tmp_int); - Serial.print(","); - - // Read 2 longs... - tmp_long=DataFlash.ReadLong(); - Serial.print(tmp_long); - Serial.print(","); - tmp_long=DataFlash.ReadLong(); - Serial.print(tmp_long); - Serial.print(";"); - - // Read the checksum... - tmp_byte1=DataFlash.ReadByte(); - tmp_byte2=DataFlash.ReadByte(); - } - Serial.println(); - } - - delay(10000); -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/keywords.txt deleted file mode 100644 index eea5025224..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/DataFlash/keywords.txt +++ /dev/null @@ -1,14 +0,0 @@ -DataFlash KEYWORD1 -Init KEYWORD2 -ReadManufacturerID KEYWORD2 -GetPage KEYWORD2 -PageErase KEYWORD2 -StartWrite KEYWORD2 -StartRead KEYWORD2 -ReadByte KEYWORD2 -ReadInt KEYWORD2 -ReadLong KEYWORD2 -WriteByte KEYWORD2 -WriteInt KEYWORD2 -WriteLong KEYWORD2 - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.cpp deleted file mode 100644 index 88f131bbe4..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.cpp +++ /dev/null @@ -1,348 +0,0 @@ -// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- -// -// Interrupt-driven serial transmit/receive library. -// -// Copyright (c) 2010 Michael Smith. All rights reserved. -// -// Receive and baudrate calculations derived from the Arduino -// HardwareSerial driver: -// -// Copyright (c) 2006 Nicholas Zambetti. All right reserved. -// -// Transmit algorithm inspired by work: -// -// Code Jose Julio and Jordi Munoz. DIYDrones.com -// -// This library 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. -// -// This library is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// Lesser General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public -// License along with this library; if not, write to the Free Software -// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -// - - -//#include "../AP_Common/AP_Common.h" -#include "FastSerial.h" -#include "WProgram.h" - -#if defined(__AVR_ATmega1280__) -# define FS_MAX_PORTS 4 -#else -# define FS_MAX_PORTS 1 -#endif - -FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS]; -FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS]; - -// Default buffer sizes -#define RX_BUFFER_SIZE 128 -#define TX_BUFFER_SIZE 64 -#define BUFFER_MAX 512 - -// Interrupt handlers ////////////////////////////////////////////////////////// - -#if 0 -#define HANDLERS(_PORT, _RXVECTOR, _TXVECTOR, _UDR) \ -SIGNAL(_RXVECTOR) \ -{ \ - unsigned char c = _UDR; \ - ports[_PORT]->receive(c); \ -} \ - \ -SIGNAL(_TXVECTOR) \ -{ \ - ports[_PORT]->transmit(); \ -} \ -struct hack - -#if defined(__AVR_ATmega8__) -HANDLERS(0, SIG_UART_RECV, SIG_UART_DATA, UDR); -#else -HANDLERS(0, SIG_USART0_RECV, SIG_USART0_DATA, UDR0); -#if defined(__AVR_ATmega1280__) -HANDLERS(1, SIG_USART1_RECV, SIG_USART1_DATA, UDR1); -HANDLERS(2, SIG_USART2_RECV, SIG_USART2_DATA, UDR2); -//HANDLERS(3, SIG_USART3_RECV, SIG_USART3_DATA, UDR3); -#endif -#endif -#endif - -// Constructor ///////////////////////////////////////////////////////////////// - -FastSerial::FastSerial(const uint8_t portNumber, - volatile uint8_t *ubrrh, - volatile uint8_t *ubrrl, - volatile uint8_t *ucsra, - volatile uint8_t *ucsrb, - volatile uint8_t *udr, - const uint8_t u2x, - const uint8_t portEnableBits, - const uint8_t portTxBits) -{ - _ubrrh = ubrrh; - _ubrrl = ubrrl; - _ucsra = ucsra; - _ucsrb = ucsrb; - _udr = udr; - _u2x = u2x; - _portEnableBits = portEnableBits; - _portTxBits = portTxBits; - - // init buffers - _rxBuffer = &__FastSerial__rxBuffer[portNumber]; - _txBuffer->head = _txBuffer->tail = 0; - _txBuffer = &__FastSerial__txBuffer[portNumber]; - _rxBuffer->head = _rxBuffer->tail = 0; - - // init stdio - fdev_setup_stream(&_fd, &FastSerial::_putchar, &FastSerial::_getchar, _FDEV_SETUP_RW); - fdev_set_udata(&_fd, this); - if (0 == portNumber) { - stdout = &_fd; // serial port 0 is always the default console - stdin = &_fd; - stderr = &_fd; - } -} - -// Public Methods ////////////////////////////////////////////////////////////// - -void FastSerial::begin(long baud) -{ - unsigned int rxb, txb; - - // If we are re-configuring an already-open port, preserve the - // existing buffer sizes. - if (_open) { - rxb = _rxBuffer->mask + 1; - txb = _txBuffer->mask + 1; - } else { - rxb = RX_BUFFER_SIZE; - txb = TX_BUFFER_SIZE; - } - - begin(baud, RX_BUFFER_SIZE, TX_BUFFER_SIZE); -} - -void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace) -{ - uint16_t ubrr; - bool use_u2x = false; - int ureg, u2; - long breg, b2, dreg, d2; - - // if we are currently open, close and restart - if (_open) - end(); - - // allocate buffers - if (!_allocBuffer(_rxBuffer, rxSpace ? : RX_BUFFER_SIZE) || - !_allocBuffer(_txBuffer, txSpace ? : TX_BUFFER_SIZE)) { - end(); - return; // couldn't allocate buffers - fatal - } - _open = true; - - // U2X mode is needed for bitrates higher than (F_CPU / 16) - if (baud > F_CPU / 16) { - use_u2x = true; - ubrr = F_CPU / (8 * baud) - 1; - } else { - // Determine whether u2x mode would give a closer - // approximation of the desired speed. - - // ubrr for non-2x mode, corresponding baudrate and delta - ureg = F_CPU / 16 / baud - 1; - breg = F_CPU / 16 / (ureg + 1); - dreg = abs(baud - breg); - - // ubrr for 2x mode, corresponding bitrate and delta - u2 = F_CPU / 8 / baud - 1; - b2 = F_CPU / 8 / (u2 + 1); - d2 = abs(baud - b2); - - // Pick the setting that gives the smallest rate - // error, preferring non-u2x mode if the delta is - // identical. - if (dreg <= d2) { - ubrr = ureg; - } else { - ubrr = u2; - use_u2x = true; - } - } - - *_ucsra = use_u2x ? _BV(_u2x) : 0; - *_ubrrh = ubrr >> 8; - *_ubrrl = ubrr; - *_ucsrb |= _portEnableBits; -} - -void FastSerial::end() -{ - *_ucsrb &= ~(_portEnableBits | _portTxBits); - - _freeBuffer(_rxBuffer); - _freeBuffer(_txBuffer); - _open = false; -} - -int -FastSerial::available(void) -{ - if (!_open) - return(-1); - return((_rxBuffer->head - _rxBuffer->tail) & _rxBuffer->mask); -} - -int -FastSerial::read(void) -{ - uint8_t c; - - // if the head and tail are equal, the buffer is empty - if (!_open || (_rxBuffer->head == _rxBuffer->tail)) - return(-1); - - // pull character from tail - c = _rxBuffer->bytes[_rxBuffer->tail]; - _rxBuffer->tail = (_rxBuffer->tail + 1) & _rxBuffer->mask; - - return(c); -} - -void -FastSerial::flush(void) -{ - // don't reverse this or there may be problems if the RX interrupt - // occurs after reading the value of _rxBuffer->head but before writing - // the value to _rxBuffer->tail; the previous value of head - // may be written to tail, making it appear as if the buffer - // don't reverse this or there may be problems if the RX interrupt - // occurs after reading the value of head but before writing - // the value to tail; the previous value of rx_buffer_head - // may be written to tail, making it appear as if the buffer - // were full, not empty. - _rxBuffer->head = _rxBuffer->tail; - - // don't reverse this or there may be problems if the TX interrupt - // occurs after reading the value of _txBuffer->tail but before writing - // the value to _txBuffer->head. - _txBuffer->tail = _rxBuffer->head; -} - -void -FastSerial::write(uint8_t c) -{ - int16_t i; - - if (!_open) // drop bytes if not open - return; - - // wait for room in the tx buffer - i = (_txBuffer->head + 1) & _txBuffer->mask; - while (i == _txBuffer->tail) - ; - - // add byte to the buffer - _txBuffer->bytes[_txBuffer->head] = c; - _txBuffer->head = i; - - // enable the data-ready interrupt, as it may be off if the buffer is empty - *_ucsrb |= _portTxBits; -} - -// STDIO emulation ///////////////////////////////////////////////////////////// - -int -FastSerial::_putchar(char c, FILE *stream) -{ - FastSerial *fs; - - fs = (FastSerial *)fdev_get_udata(stream); - if ('\n' == c) - fs->write('\r'); // ASCII translation on the cheap - fs->write(c); - return(0); -} - -int -FastSerial::_getchar(FILE *stream) -{ - FastSerial *fs; - - fs = (FastSerial *)fdev_get_udata(stream); - - // We return -1 if there is nothing to read, which the library interprets - // as an error, which our clients will need to deal with. - return(fs->read()); -} - -int -FastSerial::printf(const char *fmt, ...) -{ - va_list ap; - int i; - - va_start(ap, fmt); - i = vfprintf(&_fd, fmt, ap); - va_end(ap); - - return(i); -} - -int -FastSerial::printf_P(const char *fmt, ...) -{ - va_list ap; - int i; - - va_start(ap, fmt); - i = vfprintf_P(&_fd, fmt, ap); - va_end(ap); - - return(i); -} - -// Buffer management /////////////////////////////////////////////////////////// - -bool -FastSerial::_allocBuffer(Buffer *buffer, unsigned int size) -{ - uint8_t shift; - - // init buffer state - buffer->head = buffer->tail = 0; - - // Compute the power of 2 greater or equal to the requested buffer size - // and then a mask to simplify wrapping operations. Using __builtin_clz - // would seem to make sense, but it uses a 256(!) byte table. - // Note that we ignore requests for more than BUFFER_MAX space. - for (shift = 1; (1U << shift) < min(BUFFER_MAX, size); shift++) - ; - buffer->mask = (1 << shift) - 1; - - // allocate memory for the buffer - if this fails, we fail - buffer->bytes = (uint8_t *)malloc(buffer->mask + 1); - - return(buffer->bytes != NULL); -} - -void -FastSerial::_freeBuffer(Buffer *buffer) -{ - buffer->head = buffer->tail = 0; - buffer->mask = 0; - if (NULL != buffer->bytes) { - free(buffer->bytes); - buffer->bytes = NULL; - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.h deleted file mode 100644 index 1b83ebcca7..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/FastSerial.h +++ /dev/null @@ -1,261 +0,0 @@ -// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- -// -// Interrupt-driven serial transmit/receive library. -// -// Copyright (c) 2010 Michael Smith. All rights reserved. -// -// Receive and baudrate calculations derived from the Arduino -// HardwareSerial driver: -// -// Copyright (c) 2006 Nicholas Zambetti. All right reserved. -// -// Transmit algorithm inspired by work: -// -// Code Jose Julio and Jordi Munoz. DIYDrones.com -// -// This library 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. -// -// This library is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied -// warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR -// PURPOSE. See the GNU Lesser General Public License for more -// details. -// -// You should have received a copy of the GNU Lesser General -// Public License along with this library; if not, write to the -// Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, -// Boston, MA 02110-1301 USA -// - -// -// Note that this library does not pre-declare drivers for serial -// ports; the user must explicitly create drivers for the ports they -// wish to use. This is less friendly than the stock Arduino driver, -// but it saves ~200 bytes for every unused port. -// - -#ifndef FastSerial_h -#define FastSerial_h - -// disable the stock Arduino serial driver -#ifdef HardwareSerial_h -# error Must include FastSerial.h before the Arduino serial driver is defined. -#endif -#define HardwareSerial_h - -#include -#include -#include -#include -#include - -// -// Because Arduino libraries aren't really libraries, but we want to -// only define interrupt handlers for serial ports that are actually -// used, we have to force our users to define them using a macro. -// -// Due to the way interrupt vectors are specified, we have to have -// a separate macro for every port. Ugh. -// -// The macros are: -// -// FastSerialPort0() creates referencing serial port 0 -// FastSerialPort1() creates referencing serial port 1 -// FastSerialPort2() creates referencing serial port 2 -// FastSerialPort3() creates referencing serial port 3 -// - -// -// Forward declarations for clients that want to assume that the -// default Serial* objects exist. -// -// Note that the application is responsible for ensuring that these -// actually get defined, otherwise Arduino will suck in the -// HardwareSerial library and linking will fail. -// -extern class FastSerial Serial; -extern class FastSerial Serial1; -extern class FastSerial Serial2; -//extern class FastSerial Serial3; - - -class FastSerial : public Stream { -public: - FastSerial(const uint8_t portNumber, - volatile uint8_t *ubrrh, - volatile uint8_t *ubrrl, - volatile uint8_t *ucsra, - volatile uint8_t *ucsrb, - volatile uint8_t *udr, - const uint8_t u2x, - const uint8_t portEnableBits, - const uint8_t portTxBits); - - // Serial API - void begin(long baud); - void begin(long baud, unsigned int rxSpace, unsigned int txSpace); - void end(void); - int available(void); - int read(void); - void flush(void); - void write(uint8_t c); - using Stream::write; - - // stdio extensions - int printf(const char *fmt, ...); - int printf_P(const char *fmt, ...); - FILE *getfd(void) { return &_fd; }; - - // public so the interrupt handlers can see it - struct Buffer { - volatile uint16_t head, tail; - uint16_t mask; - uint8_t *bytes; - }; - -private: - // register accessors - volatile uint8_t *_ubrrh; - volatile uint8_t *_ubrrl; - volatile uint8_t *_ucsra; - volatile uint8_t *_ucsrb; - volatile uint8_t *_udr; - - // register magic numbers - uint8_t _portEnableBits; // rx, tx and rx interrupt enables - uint8_t _portTxBits; // tx data and completion interrupt enables - uint8_t _u2x; - - // ring buffers - Buffer *_rxBuffer; - Buffer *_txBuffer; - bool _open; - - bool _allocBuffer(Buffer *buffer, unsigned int size); - void _freeBuffer(Buffer *buffer); - - // stdio emulation - FILE _fd; - static int _putchar(char c, FILE *stream); - static int _getchar(FILE *stream); -}; - -// Used by the per-port interrupt vectors -extern FastSerial::Buffer __FastSerial__rxBuffer[]; -extern FastSerial::Buffer __FastSerial__txBuffer[]; - -// Generic Rx/Tx vectors for a serial port - needs to know magic numbers -#define FastSerialHandler(_PORT, _RXVECTOR, _TXVECTOR, _UDR, _UCSRB, _TXBITS) \ -ISR(_RXVECTOR, ISR_BLOCK) \ -{ \ - uint8_t c; \ - int16_t i; \ - \ - /* read the byte as quickly as possible */ \ - c = _UDR; \ - /* work out where the head will go next */ \ - i = (__FastSerial__rxBuffer[_PORT].head + 1) & __FastSerial__rxBuffer[_PORT].mask; \ - /* decide whether we have space for another byte */ \ - if (i != __FastSerial__rxBuffer[_PORT].tail) { \ - /* we do, move the head */ \ - __FastSerial__rxBuffer[_PORT].bytes[__FastSerial__rxBuffer[_PORT].head] = c; \ - __FastSerial__rxBuffer[_PORT].head = i; \ - } \ -} \ -ISR(_TXVECTOR, ISR_BLOCK) \ -{ \ - /* if there is another character to send */ \ - if (__FastSerial__txBuffer[_PORT].tail != __FastSerial__txBuffer[_PORT].head) { \ - _UDR = __FastSerial__txBuffer[_PORT].bytes[__FastSerial__txBuffer[_PORT].tail]; \ - /* increment the tail */ \ - __FastSerial__txBuffer[_PORT].tail = \ - (__FastSerial__txBuffer[_PORT].tail + 1) & __FastSerial__txBuffer[_PORT].mask; \ - } else { \ - /* there are no more bytes to send, disable the interrupt */ \ - if (__FastSerial__txBuffer[_PORT].head == __FastSerial__txBuffer[_PORT].tail) \ - _UCSRB &= ~_TXBITS; \ - } \ -} \ -struct hack - -// Macros defining serial ports -#if defined(__AVR_ATmega1280__) -#define FastSerialPort0(_portName) \ - FastSerial _portName(0, \ - &UBRR0H, \ - &UBRR0L, \ - &UCSR0A, \ - &UCSR0B, \ - &UDR0, \ - U2X0, \ - (_BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0)), \ - (_BV(UDRIE0))); \ - FastSerialHandler(0, SIG_USART0_RECV, SIG_USART0_DATA, UDR0, UCSR0B, _BV(UDRIE0)) -#define FastSerialPort1(_portName) \ - FastSerial _portName(1, \ - &UBRR1H, \ - &UBRR1L, \ - &UCSR1A, \ - &UCSR1B, \ - &UDR1, \ - U2X1, \ - (_BV(RXEN1) | _BV(TXEN1) | _BV(RXCIE1)), \ - (_BV(UDRIE1))); \ - FastSerialHandler(1, SIG_USART1_RECV, SIG_USART1_DATA, UDR1, UCSR1B, _BV(UDRIE1)) -#define FastSerialPort2(_portName) \ - FastSerial _portName(2, \ - &UBRR2H, \ - &UBRR2L, \ - &UCSR2A, \ - &UCSR2B, \ - &UDR2, \ - U2X2, \ - (_BV(RXEN2) | _BV(TXEN2) | _BV(RXCIE2)), \ - (_BV(UDRIE2))); \ - FastSerialHandler(2, SIG_USART2_RECV, SIG_USART2_DATA, UDR2, UCSR2B, _BV(UDRIE2)) -/* - #define FastSerialPort3(_portName) \ - FastSerial _portName(3, \ - &UBRR3H, \ - &UBRR3L, \ - &UCSR3A, \ - &UCSR3B, \ - &UDR3, \ - U2X3, \ - (_BV(RXEN3) | _BV(TXEN3) | _BV(RXCIE3)), \ - (_BV(UDRIE3))); \ - FastSerialHandler(3, SIG_USART3_RECV, SIG_USART3_DATA, UDR3, UCSR3B, _BV(UDRIE3)) -*/ - #else -#if defined(__AVR_ATmega8__) -#define FastSerialPort0(_portName) \ - FastSerial _portName(0, \ - &UBRR0H, \ - &UBRR0L, \ - &UCSR0A, \ - &UCSR0B, \ - &UDR0, \ - U2X0, \ - (_BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0)), \ - (_BV(UDRIE0))); \ - FastSerialHandler(0, SIG_UART_RECV, SIG_UART_DATA, UDR0, UCSR0B, _BV(UDRIE0)) -#else -// note no SIG_USART_* defines for the 168, 328, etc. -#define FastSerialPort0(_portName) \ - FastSerial _portName(0, \ - &UBRR0H, \ - &UBRR0L, \ - &UCSR0A, \ - &UCSR0B, \ - &UDR0, \ - U2X0, \ - (_BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0)), \ - (_BV(UDRIE0))); \ - FastSerialHandler(0, USART_RX_vect, USART_UDRE_vect, UDR0, UCSR0B, _BV(UDRIE0)) -#endif -#endif -#endif // FastSerial_h diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/examples/FastSerial/FastSerial.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/examples/FastSerial/FastSerial.pde deleted file mode 100644 index e752e5c099..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/examples/FastSerial/FastSerial.pde +++ /dev/null @@ -1,55 +0,0 @@ -// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- - -// -// Example code for the FastSerial driver. -// -// This code is placed into the public domain. -// - -// -// Include the FastSerial library header. -// -// Note that this causes the standard Arduino Serial* driver to be -// disabled. -// -#include - -// -// Create a FastSerial driver that looks just like the stock Arduino -// driver. -// -FastSerialPort0(Serial); - -// -// To create a driver for a different serial port, on a board that -// supports more than one, use the appropriate macro: -// -//FastSerialPort2(Serial2); - - -void setup(void) -{ - // - // Set the speed for our replacement serial port. - // - Serial.begin(38400); - - // - // And send a message. - // - Serial.println("begin"); -} - -void -loop(void) -{ - int c; - - // - // Perform a simple loopback operation. - // - c = Serial.read(); - if (-1 != c) - Serial.write(c); -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/keywords.txt deleted file mode 100644 index 5be403f4fa..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/FastSerial/keywords.txt +++ /dev/null @@ -1,7 +0,0 @@ -FastSerial KEYWORD1 -begin KEYWORD2 -end KEYWORD2 -available KEYWORD2 -read KEYWORD2 -flush KEYWORD2 -write KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/.DS_Store b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/.DS_Store deleted file mode 100644 index 944be92a09..0000000000 Binary files a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/.DS_Store and /dev/null differ diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.cpp deleted file mode 100644 index a087ce9516..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.cpp +++ /dev/null @@ -1,279 +0,0 @@ -/* - GPS_IMU.cpp - IMU/X-Plane GPS library for Arduino -*/ - -#include "GPS_IMU.h" -#include -#include "WProgram.h" - - -// Constructors //////////////////////////////////////////////////////////////// -GPS_IMU_Class::GPS_IMU_Class() -{ -} - - -// Public Methods ////////////////////////////////////////////////////////////// -void GPS_IMU_Class::Init(void) -{ - ck_a = 0; - ck_b = 0; - IMU_step = 0; - NewData = 0; - Fix = 0; - PrintErrors = 0; - - IMU_timer = millis(); //Restarting timer... - // Initialize serial port - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - #else - Serial.begin(38400); - #endif -} - -// optimization : This code don´t wait for data, only proccess the data available -// We can call this function on the main loop (50Hz loop) -// If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info. -void GPS_IMU_Class::Read(void) -{ - static unsigned long GPS_timer = 0; - byte data; - int numc = 0; - static byte message_num = 0; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial.available(); - #else - numc = Serial.available(); - #endif - - if (numc > 0){ - for (int i=0;i 28){ - IMU_step = 0; //Bad data, so restart to IMU_step zero and try again. - payload_counter = 0; - ck_a = 0; - ck_b = 0; - //payload_error_count++; - } - break; - - case 5: - message_num = data; - checksum(data); - IMU_step++; - break; - - case 6: // Payload data read... - // We stay in this state until we reach the payload_length - buffer[payload_counter] = data; - checksum(data); - payload_counter++; - if (payload_counter >= payload_length) { - IMU_step++; - } - break; - - case 7: - IMU_ck_a = data; // First checksum byte - IMU_step++; - break; - - case 8: - IMU_ck_b = data; // Second checksum byte - - // We end the IMU/GPS read... - // Verify the received checksum with the generated checksum.. - if((ck_a == IMU_ck_a) && (ck_b == IMU_ck_b)) { - if (message_num == 0x02) { - IMU_join_data(); - IMU_timer = millis(); - } else if (message_num == 0x03) { - GPS_join_data(); - GPS_timer = millis(); - } else if (message_num == 0x04) { - IMU2_join_data(); - IMU_timer = millis(); - } else if (message_num == 0x0a) { - //PERF_join_data(); - } else { - Serial.print("Invalid message number = "); - Serial.println(message_num,DEC); - } - } else { - Serial.println("XXX Checksum error"); //bad checksum - //imu_checksum_error_count++; - } - // Variable initialization - IMU_step = 0; - payload_counter = 0; - ck_a = 0; - ck_b = 0; - IMU_timer = millis(); //Restarting timer... - break; - } - }// End for... - } - // If we don't receive GPS packets in 2 seconds => Bad FIX state - if ((millis() - GPS_timer) > 3000){ - Fix = 0; - } - if (PrintErrors){ - Serial.println("ERR:GPS_TIMEOUT!!"); - } -} - -/**************************************************************** - * - ****************************************************************/ - -void GPS_IMU_Class::IMU_join_data(void) -{ - int j; - - - //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. - //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. - - //Storing IMU roll - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - roll_sensor = intUnion.word; - - //Storing IMU pitch - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - pitch_sensor = intUnion.word; - - //Storing IMU heading (yaw) - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - Ground_Course = intUnion.word; - imu_ok = true; -} - -void GPS_IMU_Class::IMU2_join_data() -{ - int j=0; - - //Storing IMU roll - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - roll_sensor = intUnion.word; - - //Storing IMU pitch - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - pitch_sensor = intUnion.word; - - //Storing IMU heading (yaw) - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - Ground_Course = (unsigned int)intUnion.word; - - //Storing airspeed - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - airspeed = intUnion.word; - - imu_ok = true; - -} - -void GPS_IMU_Class::GPS_join_data(void) -{ - //gps_messages_received++; - int j = 0; - - Longitude = join_4_bytes(&buffer[j]); // Lat and Lon * 10**7 - j += 4; - - Lattitude = join_4_bytes(&buffer[j]); - j += 4; - - //Storing GPS Height above the sea level - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - Altitude = (long)intUnion.word * 10; // Altitude in meters * 100 - - //Storing Speed (3-D) - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - Speed_3d = Ground_Speed = (float)intUnion.word; // Speed in M/S * 100 - - //We skip the gps ground course because we use yaw value from the IMU for ground course - j += 2; - Time = join_4_bytes(&buffer[j]); // Time of Week in milliseconds - j +=4; - imu_health = buffer[j++]; - - NewData = 1; - Fix = 1; - -} - - -/**************************************************************** - * - ****************************************************************/ - // Join 4 bytes into a long -long GPS_IMU_Class::join_4_bytes(unsigned char Buffer[]) -{ - longUnion.byte[0] = *Buffer; - longUnion.byte[1] = *(Buffer+1); - longUnion.byte[2] = *(Buffer+2); - longUnion.byte[3] = *(Buffer+3); - return(longUnion.dword); -} - - -/**************************************************************** - * - ****************************************************************/ -// checksum algorithm -void GPS_IMU_Class::checksum(byte IMU_data) -{ - ck_a+=IMU_data; - ck_b+=ck_a; -} - -GPS_IMU_Class GPS; diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h deleted file mode 100644 index fcf247b911..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/GPS_IMU.h +++ /dev/null @@ -1,69 +0,0 @@ -#ifndef GPS_IMU_h -#define GPS_IMU_h - -#include - -#define IMU_MAXPAYLOAD 32 - -class GPS_IMU_Class -{ - private: - // Internal variables - union int_union { - int16_t word; - uint8_t byte[2]; - } intUnion; - - union long_union { - int32_t dword; - uint8_t byte[4]; - } longUnion; - - uint8_t ck_a; // Packet checksum - uint8_t ck_b; - uint8_t IMU_ck_a; - uint8_t IMU_ck_b; - uint8_t IMU_step; - uint8_t IMU_class; - uint8_t message_num; - uint8_t payload_length; - uint8_t payload_counter; - uint8_t buffer[IMU_MAXPAYLOAD]; - - long IMU_timer; - long IMU_ecefVZ; - void IMU_join_data(); - void IMU2_join_data(); - void GPS_join_data(); - void checksum(unsigned char IMU_data); - long join_4_bytes(unsigned char Buffer[]); - - public: - // Methods - GPS_IMU_Class(); - void Init(); - void Read(); - - // Properties - long roll_sensor; // how much we're turning in deg * 100 - long pitch_sensor; // our angle of attack in deg * 100 - int airspeed; - float imu_health; - uint8_t imu_ok; - - long Time; //GPS Millisecond Time of Week - long Lattitude; // Geographic coordinates - long Longitude; - long Altitude; - long Ground_Speed; - long Ground_Course; - long Speed_3d; - - uint8_t NumSats; // Number of visible satelites - uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic) - uint8_t NewData; // 1:New GPS Data - uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) -}; - -extern GPS_IMU_Class GPS; -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/.DS_Store b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/.DS_Store deleted file mode 100644 index ccb9677482..0000000000 Binary files a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/.DS_Store and /dev/null differ diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde deleted file mode 100644 index 5ccd3f9dd1..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde +++ /dev/null @@ -1,44 +0,0 @@ -/* - Example of GPS IMU library. - Code by Jordi Munoz, Jose Julio and, Jason Short . DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 -*/ - -#include // GPS Library - -void setup() -{ - Serial.begin(38400); - Serial.println("GPS IMU library test"); - GPS.Init(); // GPS Initialization - delay(1000); -} -void loop() -{ - GPS.Read(); - if (1){ // New GPS data? - - Serial.print("GPS:"); - Serial.print(" Lat:"); - Serial.print(GPS.Lattitude); - Serial.print(" Lon:"); - Serial.print(GPS.Longitude); - Serial.print(" Alt:"); - Serial.print((float)GPS.Altitude/100.0); - Serial.print(" GSP:"); - Serial.print((float)GPS.Ground_Speed/100.0); - Serial.print(" COG:"); - Serial.print(GPS.Ground_Course/1000000); - Serial.print(" SAT:"); - Serial.print((int)GPS.NumSats); - Serial.print(" FIX:"); - Serial.print((int)GPS.Fix); - Serial.print(" TIM:"); - Serial.print(GPS.Time); - Serial.println(); - GPS.NewData = 0; // We have read the data - } - delay(20); -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/keywords.txt deleted file mode 100644 index 9ce6d3ae99..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_IMU/keywords.txt +++ /dev/null @@ -1,16 +0,0 @@ -GPS KEYWORD1 -GPS_IMU KEYWORD1 -Init KEYWORD2 -Read KEYWORD2 -Time KEYWORD2 -Lattitude KEYWORD2 -Longitude KEYWORD2 -Altitude KEYWORD2 -Ground_Speed KETWORD2 -Ground_Course KEYWORD2 -Speed_3d KEYWORD2 -NumSats KEYWORD2 -Fix KEYWORD2 -NewData KEYWORD2 - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.cpp deleted file mode 100644 index b5b45b30e6..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.cpp +++ /dev/null @@ -1,225 +0,0 @@ -/* - GPS_MTK.cpp - Ublox GPS library for Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) - - This library 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. - - GPS configuration : Costum protocol - Baud rate : 38400 - - Methods: - Init() : GPS Initialization - Read() : Call this funcion as often as you want to ensure you read the incomming gps data - - Properties: - Lattitude : Lattitude * 10,000,000 (long value) - Longitude : Longitude * 10,000,000 (long value) - Altitude : Altitude * 100 (meters) (long value) - Ground_speed : Speed (m/s) * 100 (long value) - Ground_course : Course (degrees) * 100 (long value) - NewData : 1 when a new data is received. - You need to write a 0 to NewData when you read the data - Fix : 0: GPS NO FIX or 2D FIX, 1: 3D FIX. - -*/ - -#include "GPS_MTK.h" -#include -#include "WProgram.h" - - -// Constructors //////////////////////////////////////////////////////////////// -GPS_MTK_Class::GPS_MTK_Class() -{ -} - - -// Public Methods ////////////////////////////////////////////////////////////// -void GPS_MTK_Class::Init(void) -{ - delay(200); - ck_a=0; - ck_b=0; - UBX_step=0; - NewData=0; - Fix=0; - PrintErrors=0; - GPS_timer=millis(); //Restarting timer... - // Initialize serial port - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - #else - Serial.begin(38400); - #endif - Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); - //Serial.println("sent config string"); -} - -// optimization : This code don´t wait for data, only proccess the data available -// We can call this function on the main loop (50Hz loop) -// If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info. -void GPS_MTK_Class::Read(void) -{ - static unsigned long GPS_timer=0; - byte data; - int numc; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial1.available(); - #else - numc = Serial.available(); - #endif - if (numc > 0) - for (int i=0;i Bad FIX state - if ((millis() - GPS_timer)>2000) - { - Fix = 0; - if (PrintErrors) - Serial.println("ERR:GPS_TIMEOUT!!"); - } -} - -/**************************************************************** - * - ****************************************************************/ -// Private Methods ////////////////////////////////////////////////////////////// -void GPS_MTK_Class::parse_ubx_gps(void) -{ - int j; -//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. -//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. - if(UBX_class==0x01) - { - switch(UBX_id)//Checking the UBX ID - { - - - case 0x05: //ID Custom - j=0; - Lattitude= join_4_bytes(&UBX_buffer[j]) * 10; // lon*10,000,000 - j+=4; - Longitude = join_4_bytes(&UBX_buffer[j]) * 10; // lat*10000000 - j+=4; - Altitude = join_4_bytes(&UBX_buffer[j]); // MSL - j+=4; - Ground_Speed = join_4_bytes(&UBX_buffer[j]); - j+=4; - Ground_Course = join_4_bytes(&UBX_buffer[j]) / 10000; // Heading 2D - j+=4; - NumSats=UBX_buffer[j]; - j++; - Fix=UBX_buffer[j]; - if (Fix==3) - Fix = 1; - else - Fix = 0; - j++; - Time = join_4_bytes(&UBX_buffer[j]); - NewData=1; - break; - - } - } -} - - -/**************************************************************** - * - ****************************************************************/ - // Join 4 bytes into a long -long GPS_MTK_Class::join_4_bytes(unsigned char Buffer[]) -{ - union long_union { - int32_t dword; - uint8_t byte[4]; -} longUnion; - - longUnion.byte[3] = *Buffer; - longUnion.byte[2] = *(Buffer+1); - longUnion.byte[1] = *(Buffer+2); - longUnion.byte[0] = *(Buffer+3); - return(longUnion.dword); -} - -/**************************************************************** - * - ****************************************************************/ -// checksum algorithm -void GPS_MTK_Class::ubx_checksum(byte ubx_data) -{ - ck_a+=ubx_data; - ck_b+=ck_a; -} - -GPS_MTK_Class GPS; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h deleted file mode 100644 index 3023b112ab..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/GPS_MTK.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef GPS_MTK_h -#define GPS_MTK_h - -#include - -#define UBX_MAXPAYLOAD 60 - -class GPS_MTK_Class -{ - private: - // Internal variables - uint8_t ck_a; // Packet checksum - uint8_t ck_b; - uint8_t UBX_step; - uint8_t UBX_class; - uint8_t UBX_id; - uint8_t UBX_payload_length_hi; - uint8_t UBX_payload_length_lo; - uint8_t UBX_payload_counter; - uint8_t UBX_buffer[UBX_MAXPAYLOAD]; - uint8_t UBX_ck_a; - uint8_t UBX_ck_b; - long GPS_timer; - long UBX_ecefVZ; - void parse_ubx_gps(); - void ubx_checksum(unsigned char ubx_data); - long join_4_bytes(unsigned char Buffer[]); - - public: - // Methods - GPS_MTK_Class(); - void Init(); - void Read(); - // Properties - long Time; //GPS Millisecond Time of Week - long Lattitude; // Geographic coordinates - long Longitude; - long Altitude; - long Ground_Speed; - long Ground_Course; - uint8_t NumSats; // Number of visible satelites - uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic) - uint8_t NewData; // 1:New GPS Data - uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) -}; - -extern GPS_MTK_Class GPS; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde deleted file mode 100644 index 6fa5a1cc2f..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde +++ /dev/null @@ -1,47 +0,0 @@ -/* - Example of GPS MTK library. - Code by Jordi Mu�oz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 -*/ - -#include // UBLOX GPS Library - -#define T6 1000000 -#define T7 10000000 - -void setup() -{ - Serial.begin(38400); - Serial.println("GPS MTK library test"); - GPS.Init(); // GPS Initialization - delay(1000); -} -void loop() -{ - GPS.Read(); - if (GPS.NewData) // New GPS data? - { - Serial.print("GPS:"); - Serial.print(" Lat:"); - Serial.print((float)GPS.Lattitude/T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)GPS.Longitude/T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)GPS.Altitude/100.0, DEC); - Serial.print(" GSP:"); - Serial.print((float)GPS.Ground_Speed/100.0, DEC); - Serial.print(" COG:"); - Serial.print(GPS.Ground_Course/100.0, DEC); - Serial.print(" SAT:"); - Serial.print((int)GPS.NumSats); - Serial.print(" FIX:"); - Serial.print((int)GPS.Fix); - Serial.print(" TIM:"); - Serial.print(GPS.Time); - Serial.println(); - GPS.NewData = 0; // We have readed the data - } - delay(20); -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/keywords.txt deleted file mode 100644 index 719ed917a1..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_MTK/keywords.txt +++ /dev/null @@ -1,16 +0,0 @@ -GPS KEYWORD1 -GPS_MTK KEYWORD1 -Init KEYWORD2 -Read KEYWORD2 -Time KEYWORD2 -Lattitude KEYWORD2 -Longitude KEYWORD2 -Altitude KEYWORD2 -Ground_Speed KEYWORD2 -Ground_Course KEYWORD2 -Speed_3d KEYWORD2 -NumSats KEYWORD2 -Fix KEYWORD2 -NewData KEYWORD2 - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.cpp deleted file mode 100644 index 0289b4adc6..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/* - GPS_NMEA.cpp - Generic NMEA GPS library for Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) - - This library 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. - - GPS configuration : NMEA protocol - Baud rate : 38400 - NMEA Sentences : - $GPGGA : Global Positioning System Fix Data - $GPVTG : Ttack and Ground Speed - - Methods: - Init() : GPS Initialization - Read() : Call this funcion as often as you want to ensure you read the incomming gps data - - Properties: - Lattitude : Lattitude * 10000000 (long value) - Longitude : Longitude * 10000000 (long value) - Altitude : Altitude * 1000 (milimeters) (long value) - Ground_speed : Speed (m/s) * 100 (long value) - Ground_course : Course (degrees) * 100 (long value) - Type : 2 (This indicate that we are using the Generic NMEA library) - NewData : 1 when a new data is received. - You need to write a 0 to NewData when you read the data - Fix : >=1: GPS FIX, 0: No Fix (normal logic) - Quality : 0 = No Fix - 1 = Bad (Num sats < 5) - 2 = Poor - 3 = Medium - 4 = Good - - NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset) -*/ - -#include "GPS_NMEA.h" - -#include -#include "WProgram.h" - - -// Constructors //////////////////////////////////////////////////////////////// -GPS_NMEA_Class::GPS_NMEA_Class() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void GPS_NMEA_Class::Init(void) -{ - Type = 2; - GPS_checksum_calc = false; - bufferidx = 0; - NewData=0; - Fix=0; - Quality=0; - PrintErrors=0; - // Initialize serial port - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - #else - Serial.begin(38400); - #endif -} - -// This code don´t wait for data, only proccess the data available on serial port -// We can call this function on the main loop (50Hz loop) -// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info. -void GPS_NMEA_Class::Read(void) -{ - char c; - int numc; - int i; - - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial1.available(); - #else - numc = Serial.available(); - #endif - if (numc > 0) - for (i=0;i Convert to decimal - Lattitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) (0.6 = 3/5) - parseptr = strchr(parseptr, ',')+1; - // - if (*parseptr=='S') - Lattitude = -1*Lattitude; // South Lattitudes are negative - // - parseptr = strchr(parseptr, ',')+1; - // W Longitudes are Negative - aux_deg = parsedecimal(parseptr,3); // degrees - aux_min = parsenumber(parseptr+3,4); // minutes (sexagesimal) - Longitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) - //Longitude = -1*Longitude; // This Assumes that we are in W longitudes... - parseptr = strchr(parseptr, ',')+1; - // - if (*parseptr=='W') - Longitude = -1*Longitude; // West Longitudes are negative - // - parseptr = strchr(parseptr, ',')+1; - Fix = parsedecimal(parseptr,1); - parseptr = strchr(parseptr, ',')+1; - NumSats = parsedecimal(parseptr,2); - parseptr = strchr(parseptr, ',')+1; - HDOP = parsenumber(parseptr,1); // HDOP * 10 - parseptr = strchr(parseptr, ',')+1; - Altitude = parsenumber(parseptr,1)*100; // Altitude in decimeters*100 = milimeters - if (Fix < 1) - Quality = 0; // No FIX - else if(NumSats<5) - Quality = 1; // Bad (Num sats < 5) - else if(HDOP>30) - Quality = 2; // Poor (HDOP > 30) - else if(HDOP>25) - Quality = 3; // Medium (HDOP > 25) - else - Quality = 4; // Good (HDOP < 25) - } - else - { - if (PrintErrors) - Serial.println("GPSERR: Checksum error!!"); - } - } - } - else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG - //Serial.println(buffer); - if (buffer[bufferidx-4]=='*'){ // Check for the "*" character - NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters - if (GPS_checksum == NMEA_check){ // Checksum validation - parseptr = strchr(buffer, ',')+1; - Ground_Course = parsenumber(parseptr,2); // Ground course in degrees * 100 - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - parseptr = strchr(parseptr, ',')+1; - Ground_Speed = parsenumber(parseptr,2)*10/36; // Convert Km/h to m/s (*100) - //GPS_line = true; - } - else - { - if (PrintErrors) - Serial.println("GPSERR: Checksum error!!"); - } - } - } - else - { - bufferidx = 0; - if (PrintErrors) - Serial.println("GPSERR: Bad sentence!!"); - } -} - - -/**************************************************************** - * - ****************************************************************/ - // Parse hexadecimal numbers -byte GPS_NMEA_Class::parseHex(char c) { - if (c < '0') - return (0); - if (c <= '9') - return (c - '0'); - if (c < 'A') - return (0); - if (c <= 'F') - return ((c - 'A')+10); -} - -// Decimal number parser -long GPS_NMEA_Class::parsedecimal(char *str,byte num_car) { - long d = 0; - byte i; - - i = num_car; - while ((str[0] != 0)&&(i>0)) { - if ((str[0] > '9') || (str[0] < '0')) - return d; - d *= 10; - d += str[0] - '0'; - str++; - i--; - } - return d; -} - -// Function to parse fixed point numbers (numdec=number of decimals) -long GPS_NMEA_Class::parsenumber(char *str,byte numdec) { - long d = 0; - byte ndec = 0; - - while (str[0] != 0) { - if (str[0] == '.'){ - ndec = 1; - } - else { - if ((str[0] > '9') || (str[0] < '0')) - return d; - d *= 10; - d += str[0] - '0'; - if (ndec > 0) - ndec++; - if (ndec > numdec) // we reach the number of decimals... - return d; - } - str++; - } - return d; -} - -GPS_NMEA_Class GPS; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.h deleted file mode 100644 index f144a35e27..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/GPS_NMEA.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef GPS_NMEA_h -#define GPS_NMEA_h - -#include - -#define GPS_BUFFERSIZE 120 - -class GPS_NMEA_Class -{ - private: - // Internal variables - uint8_t GPS_checksum; - uint8_t GPS_checksum_calc; - char buffer[GPS_BUFFERSIZE]; - int bufferidx; - - void parse_nmea_gps(void); - uint8_t parseHex(char c); - long parsedecimal(char *str,uint8_t num_car); - long parsenumber(char *str,uint8_t numdec); - - public: - // Methods - GPS_NMEA_Class(); - void Init(); - void Read(); - // Properties - long Time; //GPS Millisecond Time of Week - long Lattitude; // Geographic coordinates - long Longitude; - long Altitude; - long Ground_Speed; - long Speed_3d; //Speed (3-D) - long Ground_Course; - uint8_t Type; // Type of GPS (library used) - uint8_t NumSats; // Number of visible satelites - uint8_t Fix; // >=1:GPS FIX 0:No FIX (normal logic) - uint8_t Quality; // GPS Signal quality - uint8_t NewData; // 1:New GPS Data - uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) - int HDOP; // HDOP -}; - -extern GPS_NMEA_Class GPS; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde deleted file mode 100644 index f7a93d0c58..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde +++ /dev/null @@ -1,42 +0,0 @@ -/* - Example of GPS NMEA library. - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 -*/ - -#include // NMEA GPS Library - -void setup() -{ - Serial.begin(57600); - Serial.println("GPS NMEA library test"); - GPS.Init(); // GPS Initialization - delay(1000); -} -void loop() -{ - GPS.Read(); - if (GPS.NewData) // New GPS data? - { - Serial.print("GPS:"); - Serial.print(" Time:"); - Serial.print(GPS.Time); - Serial.print(" Fix:"); - Serial.print((int)GPS.Fix); - Serial.print(" Lat:"); - Serial.print(GPS.Lattitude); - Serial.print(" Lon:"); - Serial.print(GPS.Longitude); - Serial.print(" Alt:"); - Serial.print(GPS.Altitude/1000.0); - Serial.print(" Speed:"); - Serial.print(GPS.Ground_Speed/100.0); - Serial.print(" Course:"); - Serial.print(GPS.Ground_Course/100.0); - Serial.println(); - GPS.NewData = 0; // We have readed the data - } - delay(20); -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/keywords.txt deleted file mode 100644 index f2d3a7653d..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_NMEA/keywords.txt +++ /dev/null @@ -1,18 +0,0 @@ -GPS KEYWORD1 -GPS_NMEA KEYWORD1 -Init KEYWORD2 -Read KEYWORD2 -Type KEYWORD2 -Time KEYWORD2 -Lattitude KEYWORD2 -Longitude KEYWORD2 -Altitude KEYWORD2 -Ground_Speed KETWORD2 -Ground_Course KEYWORD2 -Speed_3d KEYWORD2 -NumSats KEYWORD2 -Fix KEYWORD2 -NewData KEYWORD2 -Quality KEYWORD2 - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.cpp deleted file mode 100644 index d1f29b336f..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.cpp +++ /dev/null @@ -1,274 +0,0 @@ -/* - GPS_UBLOX.cpp - Ublox GPS library for Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1) - - This library 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. - - GPS configuration : Ublox protocol - Baud rate : 38400 - Active messages : - NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet - NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet - NAV - STATUS Receiver Navigation Status - or - NAV - SOL Navigation Solution Information - - Methods: - Init() : GPS Initialization - Read() : Call this funcion as often as you want to ensure you read the incomming gps data - - Properties: - Lattitude : Lattitude * 10,000,000 (long value) - Longitude : Longitude * 10,000,000 (long value) - Altitude : Altitude * 100 (meters) (long value) - Ground_speed : Speed (m / s) * 100 (long value) - Ground_course : Course (degrees) * 100 (long value) - NewData : 1 when a new data is received. - You need to write a 0 to NewData when you read the data - Fix : 1: GPS FIX, 0: No Fix (normal logic) - -*/ - -#include "GPS_UBLOX.h" - -#include -#include "WProgram.h" - - -// Constructors //////////////////////////////////////////////////////////////// -GPS_UBLOX_Class::GPS_UBLOX_Class() -{ -} - - -// Public Methods ////////////////////////////////////////////////////////////// -void GPS_UBLOX_Class::Init(void) -{ - ck_a = 0; - ck_b = 0; - UBX_step = 0; - NewData = 0; - Fix = 0; - PrintErrors = 0; - GPS_timer = millis(); // Restarting timer... - // Initialize serial port - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - #else - Serial.begin(38400); - #endif -} - -// optimization : This code don´t wait for data, only proccess the data available -// We can call this function on the main loop (50Hz loop) -// If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info. -void GPS_UBLOX_Class::Read(void) -{ - static unsigned long GPS_timer = 0; - byte data; - int numc; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial1.available(); - #else - numc = Serial.available(); - #endif - if (numc > 0) - for (int i = 0; i < numc; i++) // Process bytes received - { - #if defined(__AVR_ATmega1280__) - data = Serial1.read(); - #else - data = Serial.read(); - #endif - switch(UBX_step) // Normally we start from zero. This is a state machine - { - case 0: - if(data == 0xB5) // UBX sync char 1 - UBX_step++; // OH first data packet is correct, so jump to the next step - break; - case 1: - if(data == 0x62) // UBX sync char 2 - UBX_step++; // ooh! The second data packet is correct, jump to the step 2 - else - UBX_step = 0; // Nop, is not correct so restart to step zero and try again. - break; - case 2: - UBX_class = data; - ubx_checksum(UBX_class); - UBX_step++; - break; - case 3: - UBX_id = data; - ubx_checksum(UBX_id); - UBX_step++; - break; - case 4: - UBX_payload_length_hi = data; - ubx_checksum(UBX_payload_length_hi); - UBX_step++; - // We check if the payload lenght is valid... - if (UBX_payload_length_hi >= UBX_MAXPAYLOAD) - { - if (PrintErrors) - Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!"); - UBX_step = 0; // Bad data, so restart to step zero and try again. - ck_a = 0; - ck_b = 0; - } - break; - case 5: - UBX_payload_length_lo = data; - ubx_checksum(UBX_payload_length_lo); - UBX_step++; - UBX_payload_counter = 0; - break; - case 6: // Payload data read... - if (UBX_payload_counter < UBX_payload_length_hi) // We stay in this state until we reach the payload_length - { - UBX_buffer[UBX_payload_counter] = data; - ubx_checksum(data); - UBX_payload_counter++; - if (UBX_payload_counter == UBX_payload_length_hi) - UBX_step++; - } - break; - case 7: - UBX_ck_a = data; // First checksum byte - UBX_step++; - break; - case 8: - UBX_ck_b = data; // Second checksum byte - - // We end the GPS read... - if((ck_a == UBX_ck_a) && (ck_b == UBX_ck_b)) // Verify the received checksum with the generated checksum.. - parse_ubx_gps(); // Parse the new GPS packet - else - { - if (PrintErrors) - Serial.println("ERR:GPS_CHK!!"); - } - // Variable initialization - UBX_step = 0; - ck_a = 0; - ck_b = 0; - GPS_timer = millis(); // Restarting timer... - break; - } - } // End for... - // If we don´t receive GPS packets in 2 seconds => Bad FIX state - if ((millis() - GPS_timer) > 2000) - { - Fix = 0; - if (PrintErrors) - Serial.println("ERR:GPS_TIMEOUT!!"); - } -} - -/**************************************************************** - * - ****************************************************************/ -// Private Methods ////////////////////////////////////////////////////////////// -void GPS_UBLOX_Class::parse_ubx_gps(void) -{ - int j; -//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. -//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. - if(UBX_class == 0x01) - { - switch(UBX_id) //Checking the UBX ID - { - case 0x02: // ID NAV - POSLLH - j = 0; - Time = join_4_bytes(&UBX_buffer[j]); // ms Time of week - j += 4; - Longitude = join_4_bytes(&UBX_buffer[j]); // lon * 10000000 - j += 4; - Lattitude = join_4_bytes(&UBX_buffer[j]); // lat * 10000000 - j += 4; - //Altitude = join_4_bytes(&UBX_buffer[j]); // elipsoid heigth mm - j += 4; - Altitude = (float)join_4_bytes(&UBX_buffer[j]); // MSL heigth mm - Altitude /= 10.; - - // Rescale altitude to cm - //j+=4; - /* - hacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000; - j += 4; - vacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000; - j += 4; - */ - NewData = 1; - break; - case 0x03: //ID NAV - STATUS - //if(UBX_buffer[4] >= 0x03) - if((UBX_buffer[4] >= 0x03) && (UBX_buffer[5] & 0x01)) - Fix = 1; // valid position - else - Fix = 0; // invalid position - break; - - case 0x06: //ID NAV - SOL - if((UBX_buffer[10] >= 0x03) && (UBX_buffer[11] & 0x01)) - Fix = 1; // valid position - else - Fix = 0; // invalid position - UBX_ecefVZ = join_4_bytes(&UBX_buffer[36]); // Vertical Speed in cm / s - NumSats = UBX_buffer[47]; // Number of sats... - break; - - case 0x12: // ID NAV - VELNED - j = 16; - Speed_3d = join_4_bytes(&UBX_buffer[j]); // cm / s - j += 4; - Ground_Speed = join_4_bytes(&UBX_buffer[j]); // Ground speed 2D cm / s - j += 4; - Ground_Course = join_4_bytes(&UBX_buffer[j]); // Heading 2D deg * 100000 - Ground_Course /= 1000; // Rescale heading to deg * 100 - j += 4; - /* - sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy - j += 4; - headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy - j += 4; - */ - break; - } - } -} - - -/**************************************************************** - * - ****************************************************************/ - // Join 4 bytes into a long -long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[]) -{ - union long_union { - int32_t dword; - uint8_t byte[4]; -} longUnion; - - longUnion.byte[0] = *Buffer; - longUnion.byte[1] = *(Buffer + 1); - longUnion.byte[2] = *(Buffer + 2); - longUnion.byte[3] = *(Buffer + 3); - return(longUnion.dword); -} - -/**************************************************************** - * - ****************************************************************/ -// Ublox checksum algorithm -void GPS_UBLOX_Class::ubx_checksum(byte ubx_data) -{ - ck_a += ubx_data; - ck_b += ck_a; -} - -GPS_UBLOX_Class GPS; \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.h deleted file mode 100644 index 8a9e09e611..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/GPS_UBLOX.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef GPS_UBLOX_h -#define GPS_UBLOX_h - -#include - -#define UBX_MAXPAYLOAD 60 - -class GPS_UBLOX_Class -{ - private: - // Internal variables - uint8_t ck_a; // Packet checksum - uint8_t ck_b; - uint8_t UBX_step; - uint8_t UBX_class; - uint8_t UBX_id; - uint8_t UBX_payload_length_hi; - uint8_t UBX_payload_length_lo; - uint8_t UBX_payload_counter; - uint8_t UBX_buffer[UBX_MAXPAYLOAD]; - uint8_t UBX_ck_a; - uint8_t UBX_ck_b; - long GPS_timer; - long UBX_ecefVZ; - void parse_ubx_gps(); - void ubx_checksum(unsigned char ubx_data); - long join_4_bytes(unsigned char Buffer[]); - - public: - // Methods - GPS_UBLOX_Class(); - void Init(); - void Read(); - // Properties - long Time; //GPS Millisecond Time of Week - long Lattitude; // Geographic coordinates - long Longitude; - long Altitude; - long Ground_Speed; - long Speed_3d; //Speed (3-D) - long Ground_Course; - uint8_t NumSats; // Number of visible satelites - uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic) - uint8_t NewData; // 1:New GPS Data - uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) -}; - -extern GPS_UBLOX_Class GPS; - -#endif \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde deleted file mode 100644 index 99975c7334..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde +++ /dev/null @@ -1,42 +0,0 @@ -/* - Example of GPS UBLOX library. - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 -*/ - -#include // UBLOX GPS Library - -void setup() -{ - Serial.begin(57600); - Serial.println("GPS UBLOX library test"); - GPS.Init(); // GPS Initialization - delay(1000); -} -void loop() -{ - GPS.Read(); - if (GPS.NewData) // New GPS data? - { - Serial.print("GPS:"); - Serial.print(" Time:"); - Serial.print(GPS.Time); - Serial.print(" Fix:"); - Serial.print((int)GPS.Fix); - Serial.print(" Lat:"); - Serial.print(GPS.Lattitude); - Serial.print(" Lon:"); - Serial.print(GPS.Longitude); - Serial.print(" Alt:"); - Serial.print(GPS.Altitude/1000.0); - Serial.print(" Speed:"); - Serial.print(GPS.Ground_Speed/100.0); - Serial.print(" Course:"); - Serial.print(GPS.Ground_Course/100000.0); - Serial.println(); - GPS.NewData = 0; // We have readed the data - } - delay(20); -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/keywords.txt deleted file mode 100644 index e211693803..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/GPS_UBLOX/keywords.txt +++ /dev/null @@ -1,16 +0,0 @@ -GPS KEYWORD1 -GPS_UBLOX KEYWORD1 -Init KEYWORD2 -Read KEYWORD2 -Time KEYWORD2 -Lattitude KEYWORD2 -Longitude KEYWORD2 -Altitude KEYWORD2 -Ground_Speed KETWORD2 -Ground_Course KEYWORD2 -Speed_3d KEYWORD2 -NumSats KEYWORD2 -Fix KEYWORD2 -NewData KEYWORD2 - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp deleted file mode 100644 index 25d49da553..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/JETI_Box.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/* - JETI_Box.cpp, Version 1.0 beta - Oct 2010, by Uwe Gartmann - - - written for Arduino Mega / ArduPilot Mega - -*/ -extern "C" { - #include - #include "Print.h" - #include - #include - #include "wiring.h" -} - -#include -#define LCDMaxPos 32 -#define LCDBufferSize LCDMaxPos + 1 - -volatile uint8_t lcd[LCDBufferSize]; -volatile uint8_t buttons; -volatile uint8_t lastbuttons; -volatile uint8_t cursor; -volatile uint8_t sendpos; -volatile uint8_t dmyError; -volatile uint8_t dmyBit9; -volatile uint8_t dmyData; -volatile uint8_t ledA; -volatile uint8_t t0cntr; -//volatile long valX; - -ISR(USART3_RX_vect) //serial data available -{ - // catch response from Jetibox - dmyError = UCSR3A; - dmyBit9 = UCSR3B; - dmyData = UDR3; - if (!(dmyError & ((1<>4) xor 0x0F; - t0cntr = 10; // set 10ms delay - } - } - } -} - -ISR(USART3_UDRE_vect) //transmit buffer empty -/* - * sendpos = 0 -> start sending data with this interrupt enabled, start byte with 9.bit=0 - * sendpos = 1-32 -> send display data with 9.bit=1 - * sendpos = 33 -> send end byte with 9.bit=0 - * sendpos = 34 -> set sendpos=0 and disable this interrupt - */ -{ - switch (sendpos) - { - case 0: - // send start byte with 9.bit=0 - UCSR3B &= ~(1< 0) --valX; -} - -JETI_Box_class::JETI_Box_class() -{ -// Class constructor -} - -// Public Methods -------------------------------------------------------------------- -void JETI_Box_class::begin(void) -{ -#ifndef F_CPU - #define F_CPU 16000000 -#endif -#define _UBRR3 (F_CPU/8/9600-1) //when U2X0 is not set use divider 16 instead of 8 - - // Set serial interface - // Baudrate - UCSR3A = (1<>8); //high byte - UBRR3L=_UBRR3; //low byte - // Set data format: 9data#1, odd parity, 1stop bit - UCSR3C = (0< - #include "Print.h" - #include - #include - #include "wiring.h" -} - -#include -#define LCDLine1 1 -#define LCDLine2 17 -#define LCDMaxPos 32 -#define LCDBufferSize LCDMaxPos + 1 - -volatile uint8_t lcd[LCDBufferSize]; -volatile uint8_t buttons; -volatile uint8_t lastbuttons; -volatile uint8_t cursor; -volatile uint8_t sendpos; -volatile uint8_t dummy; - -ISR(USART3_RX_vect) //serial data available -{ - // catch response from Jetibox - if (!(UCSR3A & ((1<>4) xor 0x0F; - } - } - } -} - -ISR(USART3_UDRE_vect) //transmit buffer empty -/* - * sendpos = 0 -> start sending data with this interrupt enabled, start byte with 9.bit=0 - * sendpos = 1-32 -> send display data with 9.bit=1 - * sendpos = 33 -> send end byte with 9.bit=0 - * sendpos = 34 -> set sendpos=0 and disable this interrupt - */ -{ - switch (sendpos) - { - case 0: - // send start byte with 9.bit=0 - UCSR3B &= ~(1<>8); //high byte - UBRR3L=_UBRR3; //low byte - - // Set data format: 9data#1, odd parity, 1stop bit - UCSR3C = (0< -//#include "Print.h" - -#define LCDLine1 1 -#define LCDLine2 17 - -#define JB_key_up 0b0010 -#define JB_key_right 0b0001 -#define JB_key_down 0b0100 -#define JB_key_left 0b1000 - -class JETI_Box_class : public Print { -public: - uint8_t readbuttons(void); - //long checkvalue(long v); - virtual void write(uint8_t c); - using Print::write; // pull in write(str) and write(buf, size) from Print - JETI_Box_class(); - void begin(); - void setcursor(uint8_t p); - void clear(); - void clear(uint8_t l); -}; - -extern JETI_Box_class JB; - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/keywords.txt deleted file mode 100644 index 699b80af69..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/JETI_Box/keywords.txt +++ /dev/null @@ -1,8 +0,0 @@ -JETI_Box KEYWORD1 -Begin KEYWORD2 -Refresh KEYWORD2 -Write KEYWORD2 -Print KEYWORD2 -Line1 KEYWORD2 -Line2 KEYWORD2 -Keys KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp deleted file mode 100644 index d9b4882b21..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.cpp +++ /dev/null @@ -1,327 +0,0 @@ -#ifdef __AVR_ATmega1280__ -/* - APM2_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library 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. - - RC Input : PPM signal on IC4 pin - RC Output : 11 Servo outputs (standard 20ms frame) - - Methods: - Init() : Initialization of interrupts an Timers - OutpuCh(ch, pwm) : Output value to servos (range : 900 - 2100us) ch = 0..10 - InputCh(ch) : Read a channel input value. ch = 0..7 - GetState() : Returns the state of the input. 1 => New radio frame to process - Automatically resets when we call InputCh to read channels - -*/ - -#include "APM2_RC.h" - -#define REVERSE 3050 - -// Variable definition for Input Capture interrupt -volatile uint16_t ICR4_old; -volatile uint8_t PPM_Counter = 0; -volatile uint16_t raw[8] = {1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200}; - -// Constructors //////////////////////////////////////////////////////////////// -APM2_RC::APM2_RC() -{ - _direction_mask = 255; // move to super class - -} - -void -APM2_RC::init() -{ - // Init PWM Timer 1 - pinMode(11, OUTPUT); // (PB5 / OC1A) - pinMode(12, OUTPUT); // OUT2 (PB6 / OC1B) - pinMode(13, OUTPUT); // OUT3 (PB7 / OC1C) - - // Timer 3 - pinMode(2, OUTPUT); // OUT7 (PE4 / OC3B) - pinMode(3, OUTPUT); // OUT6 (PE5 / OC3C) - pinMode(4, OUTPUT); // (PE3 / OC3A) - - // Timer 5 - pinMode(44, OUTPUT); // OUT1 (PL5 / OC5C) - pinMode(45, OUTPUT); // OUT0 (PL4 / OC5B) - pinMode(46, OUTPUT); // (PL3 / OC5A) - - // Init PPM input and PWM Timer 4 - pinMode(49, INPUT); // ICP4 pin (PL0) (PPM input) - pinMode(7, OUTPUT); // OUT5 (PH4 / OC4B) - pinMode(8, OUTPUT); // OUT4 (PH5 / OC4C) - - //Remember the registers not declared here remains zero by default... - TCCR1A =((1 << WGM11) | (1 << COM1A1) | (1 << COM1B1) | (1 << COM1C1)); // Please read page 131 of DataSheet, we are changing the registers settings of WGM11, COM1B1, COM1A1 to 1 thats all... - TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet - OCR1A = 3000; // PB5, none - //OCR1B = 3000; // PB6, OUT2 - //OCR1C = 3000; // PB7 OUT3 - ICR1 = 40000; // 50hz freq...Datasheet says (system_freq / prescaler) / target frequency. So (16000000hz / 8) / 50hz = 40000, - - // Init PWM Timer 3 - TCCR3A =((1 << WGM31) | (1 << COM3A1) | (1 << COM3B1) | (1 << COM3C1)); - TCCR3B = (1 << WGM33) | (1 << WGM32) | (1 << CS31); - OCR3A = 3000; // PE3, NONE - //OCR3B = 3000; // PE4, OUT7 - //OCR3C = 3000; // PE5, OUT6 - ICR3 = 40000; // 50hz freq - - - // Init PWM Timer 5 - TCCR5A =((1 << WGM51) | (1 << COM5A1) | (1 << COM5B1) | (1 << COM5C1)); - TCCR5B = (1 << WGM53) | (1 << WGM52) | (1 << CS51); - OCR5A = 3000; // PL3, - //OCR5B = 3000; // PL4, OUT0 - //OCR5C = 3000; // PL5, OUT1 - ICR5 = 40000; // 50hz freq - - - // Init PPM input and PWM Timer 4 - TCCR4A = ((1 << WGM40) | (1 << WGM41) | (1 << COM4C1) | (1 << COM4B1) | (1 << COM4A1)); - TCCR4B = ((1 << WGM43) | (1 << WGM42) | (1 << CS41) | (1 << ICES4)); - OCR4A = 40000; // /50hz freq. - //OCR4B = 3000; // PH4, OUT5 - //OCR4C = 3000; // PH5, OUT4 - - //TCCR4B |=(1<= radio_trim[i]) - servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0; - else - servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0; - } - servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0; - servo_in[CH3] = constrain(servo_out[CH3], 0, 100); -} - -void -APM2_RC::output() -{ - uint16_t out; - for (uint8_t i = 0; i < 8; i++){ - if (i == 3) continue; - if(radio_in[i] >= radio_trim[i]) - out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; - else - out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; - set_ch_pwm(i, out); - } - - out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0; - out += radio_min[CH3]; - set_ch_pwm(CH3, out); -} - -void -APM2_RC::trim() -{ - uint8_t temp = _mix_mode; - _mix_mode = 0; - read(); - _mix_mode = temp; - - // Store the trim values - // --------------------- - for (int y = 0; y < 8; y++) - radio_trim[y] = radio_in[y]; -} -void -APM2_RC::twitch_servos(uint8_t times) -{ - // todo -} -void -APM2_RC::set_ch_pwm(uint8_t ch, uint16_t pwm) -{ - //pwm = constrain(pwm, MIN_PULSEWIDTH, MAX_PULSEWIDTH); - - switch(ch){ - case 0: - //Serial.print("\tpwm out "); - //Serial.print(pwm,DEC); - - if((_direction_mask & 1) == 0 ) - pwm = REVERSE - pwm; - - //Serial.print("\tpwm out "); - //Serial.println(pwm,DEC); - - OCR5B = pwm << 1; - break; // ch0 - - case 1: - if((_direction_mask & 2) == 0 ) - pwm = REVERSE - pwm; - OCR5C = pwm << 1; - break; // ch0 - - case 2: - if((_direction_mask & 4) == 0 ) - pwm = REVERSE - pwm; - OCR1B = pwm << 1; - break; // ch0 - - case 3: - if((_direction_mask & 8) == 0 ) - pwm = REVERSE - pwm; - OCR1C = pwm << 1; - break; // ch0 - - case 4: - if((_direction_mask & 16) == 0 ) - pwm = REVERSE - pwm; - OCR4C = pwm << 1; - break; // ch0 - - case 5: - if((_direction_mask & 32) == 0 ) - pwm = REVERSE - pwm; - OCR4B = pwm << 1; - break; // ch0 - - case 6: - if((_direction_mask & 64) == 0 ) - pwm = REVERSE - pwm; - OCR3C = pwm << 1; - break; // ch0 - - case 7: - if((_direction_mask & 128) == 0 ) - pwm = REVERSE - pwm; - OCR3B = pwm << 1; - break; // ch0 - - case 8: - OCR5A = pwm << 1; - break; // ch0 - - case 9: - OCR1A = pwm << 1; - break; // ch0 - - case 10: - OCR3A = pwm << 1; - break; // ch0 - } -} - -/**************************************************** - Input Capture Interrupt ICP4 => PPM signal read - ****************************************************/ -ISR(TIMER4_CAPT_vect) -{ - uint16_t pulse; - uint16_t pulse_width; - - pulse = ICR4; - if (pulse < ICR4_old){ // Take care of the overflow of Timer4 (TOP = 40000) - pulse_width = (pulse + 40000) - ICR4_old; // Calculating pulse - }else{ - pulse_width = pulse - ICR4_old; // Calculating pulse - } - ICR4_old = pulse; - - - if (pulse_width > 8000){ // SYNC pulse - PPM_Counter = 0; - } else { - //PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) - raw[PPM_Counter++] = pulse_width >> 1; // Saving pulse. - } -} - - - -// InstantPWM implementation -// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use -void APM2_RC::force_out_0_1(void) -{ - if (TCNT5 > 5000) // We take care that there are not a pulse in the output - TCNT5 = 39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 -} -// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use -void APM2_RC::force_out_2_3(void) -{ - if (TCNT1 > 5000) - TCNT1 = 39990; -} - -// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use -void APM2_RC::force_out_6_7(void) -{ - if (TCNT3 > 5000) - TCNT3 = 39990; -} -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.h deleted file mode 100644 index 5947a91130..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/APM2_RC.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef APM2_RC_h -#define APM2_RC_h - -#include -#include "WProgram.h" -#include "RC.h" - -class APM2_RC : public RC -{ - public: - APM2_RC(); - void init(); - void read(); - void output(); - void set_ch_pwm(uint8_t ch, uint16_t pwm); - void trim(); - void twitch_servos(uint8_t times); - - void force_out_0_1(void); - void force_out_2_3(void); - void force_out_6_7(void); - - int16_t radio_in[8]; - int16_t radio_min[8]; - int16_t radio_trim[8]; - int16_t radio_max[8]; - - int16_t servo_in[8]; - float servo_out[8]; - - private: - uint16_t _timer_out; -}; - -#endif - - -//volatile uint16_t raw[8] = {1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200}; diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp deleted file mode 100644 index 1915285790..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.cpp +++ /dev/null @@ -1,329 +0,0 @@ -/* - AP_RC.cpp - Radio library for Arduino - Code by Jason Short. DIYDrones.com - - This library 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. - -*/ - -#include "AP_RC.h" -#include -#define REVERSE 3050 - -// Variable definition for interrupt -volatile uint16_t timer1count = 0; -volatile uint16_t timer2count = 0; -volatile uint16_t timer3count = 0; -volatile uint16_t timer4count = 0; - -volatile int16_t timer1diff = 1500 * 2; -volatile int16_t timer2diff = 1500 * 2; -volatile int16_t timer3diff = 1100 * 2; -volatile int16_t timer4diff = 1500 * 2; - -//volatile uint16_t raw[8]; - -#define CH1_READ 1 -#define CH2_READ 2 -#define CH3_READ 4 -#define CH4_READ 8 - -volatile int8_t _rc_ch_read = 0; -volatile uint8_t _timer_ovf_a = 0; -volatile uint8_t _timer_ovf_b = 0; -volatile uint8_t _timer_ovf = 0; - - -AP_RC::AP_RC() -{ - _direction_mask = 255; // move to super class - pinMode(2,INPUT); // PD2 - INT0 - CH 1 in - pinMode(3,INPUT); // PD3 - INT1 - CH 2 in - pinMode(11,INPUT); // PB3 - MOSI/OC2 - CH 3 in - pinMode(13,INPUT); // PB5 - SCK - CH 4 in - - pinMode(10,OUTPUT); // PB2 - OC1B - CH 1 out - pinMode(8, OUTPUT); // PB0 - AIN1 - CH 3 out - pinMode(9, OUTPUT); // PB1 - OC1A - CH 2 out - DDRC |= B00010000; // PC4 - - CH 4 out -} - -void -AP_RC::init() -{ - // enable pin change interrupt 2 - PCINT23..16 - PCICR = _BV(PCIE2); - // enable pin change interrupt 0 - PCINT7..0 - PCICR |= _BV(PCIE0); - // enable in change interrupt on PB5 (digital pin 13) - PCMSK0 = _BV(PCINT3) | _BV(PCINT5); - // enable pin change interrupt on PD2,PD3 (digital pin 2,3) - PCMSK2 = _BV(PCINT18) | _BV(PCINT19); - - // Timer 1 - TCCR1A = ((1 << WGM11) | (1 << COM1B1) | (1 << COM1A1)); - TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); - // Loop value - ICR1 = 40000; - - // Throttle; - // Setting up the Timer 2 - 8 bit timer - TCCR2A = 0x0; // Normal Mode - TCCR2B = _BV(CS21) |_BV(CS20); //prescaler 32, at 16mhz (32/16) = 2, the counter will increment 1 every 2us - - // trim out the radio - for(int c = 0; c < 50; c++){ - delay(20); - read(); - } - - trim(); - - for(int y = 0; y < 4; y++) { - set_ch_pwm(y, radio_trim[y]); - } - - // enable throttle and Ch4 output - TIMSK1 |= _BV(ICIE1); // Timer / Counter1, Input Capture Interrupt Enable // PB0 - output throttle - TIMSK2 = _BV(TOIE1) | _BV(OCIE2A) | _BV(OCIE2B); // Timer / Counter2 Compare Match A -} - -void -AP_RC::read() -{ - if((_direction_mask & 1) == 0 ) - timer1diff = REVERSE - timer1diff; - if((_direction_mask & 2) == 0 ) - timer2diff = REVERSE - timer2diff; - if((_direction_mask & 4) == 0 ) - timer3diff = REVERSE - timer3diff; - if((_direction_mask & 8) == 0 ) - timer4diff = REVERSE - timer4diff; - - if(_mix_mode == 1){ - // elevons - int16_t ailerons = (timer1diff - radio_trim[CH1]) * .3; - int16_t elevator = (timer2diff - radio_trim[CH2]) * .7; - - radio_in[CH1] = (elevator - ailerons); // left - radio_in[CH2] = (elevator + ailerons); // right - - radio_in[CH1] += radio_trim[CH1]; - radio_in[CH2] += radio_trim[CH2]; - - //Serial.print("radio_in[CH1] "); - //Serial.print(radio_in[CH1],DEC); - //Serial.print(" \tradio_in[CH2] "); - //Serial.println(radio_in[CH2],DEC); - - }else{ - // normal - radio_in[CH1] = timer1diff; - radio_in[CH2] = timer2diff; - } - - radio_in[CH3] = (float)radio_in[CH3] * .9 + timer3diff * .1; - radio_in[CH4] = timer4diff; - - check_throttle_failsafe(radio_in[CH3]); - - // output servos - for (uint8_t i = 0; i < 4; i++){ - if (i == 3) continue; - if(radio_in[i] >= radio_trim[i]) - servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0; - else - servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0; - } - servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0; - servo_in[CH3] = constrain(servo_out[CH3], 0, 100); -} - -void -AP_RC::output() -{ - uint16_t out; - for (uint8_t i = 0; i < 4; i++){ - if (i == 3) continue; - if(radio_in[i] >= radio_trim[i]) - out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; - else - out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; - set_ch_pwm(i, out); - } - - out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0; - out += radio_min[CH3]; - set_ch_pwm(CH3, out); -} - -void -AP_RC::trim() -{ - uint8_t temp = _mix_mode; - _mix_mode = 0; - read(); - _mix_mode = temp; - - radio_trim[CH1] = radio_in[CH1]; - radio_trim[CH2] = radio_in[CH2]; - radio_trim[CH3] = radio_in[CH3]; - radio_trim[CH4] = radio_in[CH4]; - - //Serial.print("trim "); - //Serial.println(radio_trim[CH1], DEC); -} - -void -AP_RC::twitch_servos(uint8_t times) -{ - while (times > 0){ - set_ch_pwm(CH1, radio_trim[CH1] + 100); - set_ch_pwm(CH2, radio_trim[CH2] + 100); - delay(400); - set_ch_pwm(CH1, radio_trim[CH1] - 100); - set_ch_pwm(CH2, radio_trim[CH2] - 100); - delay(200); - set_ch_pwm(CH1, radio_trim[CH1]); - set_ch_pwm(CH2, radio_trim[CH2]); - delay(30); - times--; - } -} - -void -AP_RC::set_ch_pwm(uint8_t ch, uint16_t pwm) -{ - switch(ch){ - case CH1: - if((_direction_mask & 1) == 0 ) - pwm = REVERSE - pwm; - pwm <<= 1; - OCR1A = pwm; - break; - - case CH2: - if((_direction_mask & 2) == 0 ) - pwm = REVERSE - pwm; - pwm <<= 1; - OCR1B = pwm; - break; - - case CH3: - if((_direction_mask & 4) == 0 ) - pwm = REVERSE - pwm; - // Jason's fancy 2µs hack - _timer_out = pwm % 512; - _timer_ovf_a = pwm / 512; - _timer_out >>= 1; - OCR2A = _timer_out; - break; - - case CH4: - if((_direction_mask & 8) == 0 ) - pwm = REVERSE - pwm; - _timer_out = pwm % 512; - _timer_ovf_b = pwm / 512; - _timer_out >>= 1; - OCR2B = _timer_out; - break; - } -} - - - -// radio PWM input timers -ISR(PCINT2_vect) { - int cnt = TCNT1; - if(PIND & B00000100){ // ch 1 (pin 2) is high - if ((_rc_ch_read & CH1_READ) != CH1_READ){ - _rc_ch_read |= CH1_READ; - timer1count = cnt; - } - }else if ((_rc_ch_read & CH1_READ) == CH1_READ){ // ch 1 (pin 2) is Low, and we were reading - _rc_ch_read &= B11111110; - if (cnt < timer1count) // Timer1 reset during the read of this pulse - timer1diff = (cnt + 40000 - timer1count) >> 1; // Timer1 TOP = 40000 - else - timer1diff = (cnt - timer1count) >> 1; - } - - if(PIND & B00001000){ // ch 2 (pin 3) is high - if ((_rc_ch_read & CH2_READ) != CH2_READ){ - _rc_ch_read |= CH2_READ; - timer2count = cnt; - } - }else if ((_rc_ch_read & CH2_READ) == CH2_READ){ // ch 1 (pin 2) is Low - _rc_ch_read &= B11111101; - if (cnt < timer2count) // Timer1 reset during the read of this pulse - timer2diff = (cnt + 40000 - timer2count) >> 1; // Timer1 TOP = 40000 - else - timer2diff = (cnt - timer2count) >> 1; - } -} - -ISR(PCINT0_vect) -{ - int cnt = TCNT1; - if(PINB & 8){ // pin 11 - if ((_rc_ch_read & CH3_READ) != CH3_READ){ - _rc_ch_read |= CH3_READ; - timer3count = cnt; - } - }else if ((_rc_ch_read & CH3_READ) == CH3_READ){ // ch 1 (pin 2) is Low - _rc_ch_read &= B11111011; - if (cnt < timer3count) // Timer1 reset during the read of this pulse - timer3diff = (cnt + 40000 - timer3count) >> 1; // Timer1 TOP = 40000 - else - timer3diff = (cnt - timer3count) >> 1; - } - - if(PINB & 32){ // pin 13 - if ((_rc_ch_read & CH4_READ) != CH4_READ){ - _rc_ch_read |= CH4_READ; - timer4count = cnt; - } - }else if ((_rc_ch_read & CH4_READ) == CH4_READ){ // ch 1 (pin 2) is Low - _rc_ch_read &= B11110111; - if (cnt < timer4count) // Timer1 reset during the read of this pulse - timer4diff = (cnt + 40000 - timer4count) >> 1; // Timer1 TOP = 40000 - else - timer4diff = (cnt - timer4count) >> 1; - } -} - - - -// Throttle Timer Interrupt -// ------------------------ -ISR(TIMER1_CAPT_vect) // Timer/Counter1 Capture Event -{ - //This is a timer 1 interrupts, executed every 20us - PORTB |= B00000001; //Putting the pin high! - PORTC |= B00010000; //Putting the pin high! - TCNT2 = 0; //restarting the counter of timer 2 - _timer_ovf = 0; -} - -ISR(TIMER2_OVF_vect) -{ - _timer_ovf++; -} - -ISR(TIMER2_COMPA_vect) // Timer/Counter2 Compare Match A -{ - if(_timer_ovf == _timer_ovf_a){ - PORTB &= B11111110; //Putting the pin low - } -} - -ISR(TIMER2_COMPB_vect) // Timer/Counter2 Compare Match B Rudder Servo -{ - if(_timer_ovf == _timer_ovf_b){ - PORTC &= B11101111; //Putting the pin low! - } -} - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.h deleted file mode 100644 index 8118f980fd..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/AP_RC.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef AP_RC_h -#define AP_RC_h - -#include -#include "WProgram.h" -#include "RC.h" - -class AP_RC : public RC -{ - public: - AP_RC(); - void init(); - void read(); - void output(); - void set_ch_pwm(uint8_t ch, uint16_t pwm); - void trim(); - void twitch_servos(uint8_t times); - - int16_t radio_in[4]; - int16_t radio_min[4]; - int16_t radio_trim[4]; - int16_t radio_max[4]; - - int16_t servo_in[4]; - float servo_out[4]; - - private: - uint16_t _timer_out; -}; - -#endif - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/RC.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/RC.cpp deleted file mode 100644 index 0f92dfd5b7..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/RC.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* - AP_RC.cpp - Radio library for Arduino - Code by Jason Short. DIYDrones.com - - This library 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. - -*/ - - - -#include "RC.h" - -/* -RC::RC()// : - _direction_mask(255) -{ -} -*/ - -// direction mask: 0 = normal, 1 = reversed servos -void -RC::set_channel_direction(uint8_t ch, int8_t dir) -{ - uint8_t bitflip = 1 << ch; - - if(dir == 1){ - _direction_mask |= bitflip; - }else{ - _direction_mask &= ~bitflip; - } -} - -void -RC::set_failsafe(uint16_t v) -{ - _fs_value = v; -} - -void -RC::set_mix_mode(uint8_t m) -{ - _mix_mode = m; -} - - -void -RC::check_throttle_failsafe(uint16_t throttle) -{ - //check for failsafe and debounce funky reads - // ------------------------------------------ - if (throttle < _fs_value){ - // we detect a failsafe from radio - // throttle has dropped below the mark - _fs_counter++; - if (_fs_counter == 9){ - - }else if(_fs_counter == 10) { - failsafe = 1; - }else if (_fs_counter > 10){ - _fs_counter = 11; - } - - }else if(_fs_counter > 0){ - // we are no longer in failsafe condition - // but we need to recover quickly - _fs_counter--; - if (_fs_counter > 3){ - _fs_counter = 3; - } - if (_fs_counter == 1){ - - }else if(_fs_counter == 0) { - failsafe = 0; - }else if (_fs_counter <0){ - _fs_counter = -1; - } - } -} - - - diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/RC.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/RC.h deleted file mode 100644 index fdc4505e7d..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/RC.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef RC_h -#define RC_h - -#include -#include "WProgram.h" - -#define CH1 0 -#define CH2 1 -#define CH3 2 -#define CH4 3 -#define CH5 4 -#define CH6 5 -#define CH7 6 -#define CH8 7 - -#define MIN_PULSEWIDTH 900 -#define MAX_PULSEWIDTH 2100 - -#define ELEVONS 1 - -class RC -{ - public: - // RC(); - virtual void init(); - virtual void trim(); - virtual void read(); - virtual void output(); - virtual void set_channel_direction(uint8_t ch, int8_t dir); - virtual void set_ch_pwm(uint8_t ch, uint16_t pwm); - virtual void twitch_servos(void); - - void set_failsafe(uint16_t fs); - void set_mix_mode(uint8_t mode); - - uint8_t failsafe; - - protected: - void check_throttle_failsafe(uint16_t throttle); - uint8_t _fs_counter; - uint8_t _mix_mode; // 0 = normal, 1 = elevons - uint8_t _direction_mask; - uint16_t _fs_value; // PWM value to trigger failsafe flag -}; - -#endif diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde deleted file mode 100644 index 196fafc00a..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde +++ /dev/null @@ -1,59 +0,0 @@ -/* - Example of APM2_RC library. - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com - - Print Input values and send Output to the servos - (Works with last PPM_encoder firmware) -*/ - -#include // ArduPilot Mega RC Library -APM2_RC rc; - -#define CH_ROLL 0 -#define CH_PITCH 1 -#define CH_THROTTLE 2 -#define CH_RUDDER 3 - -void setup() -{ - Serial.begin(38400); - Serial.println("ArduPilot Mega RC library test"); - - //rc.set_channel_direction(CH_ROLL, -1); - //rc.set_channel_direction(CH_PITCH, -1); - //rc.set_channel_direction(CH_THROTTLE, -1); - //rc.set_channel_direction(CH_RUDDER, -1); - rc.set_mix_mode(1); // 1 = elevons, 0 = normal - rc.init(); - delay(1000); -} - -void loop() -{ - delay(20); - rc.read_pwm(); - for(int y = 0; y < 8; y++) { - rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos - } - //print_pwm(); -} - -void print_pwm() -{ - Serial.print("ch1 "); - Serial.print(rc.radio_in[CH_ROLL], DEC); - Serial.print("\tch2: "); - Serial.print(rc.radio_in[CH_PITCH], DEC); - Serial.print("\tch3 :"); - Serial.print(rc.radio_in[CH_THROTTLE], DEC); - Serial.print("\tch4 :"); - Serial.print(rc.radio_in[CH_RUDDER], DEC); - Serial.print("\tch5 "); - Serial.print(rc.radio_in[4], DEC); - Serial.print("\tch6: "); - Serial.print(rc.radio_in[5], DEC); - Serial.print("\tch7 :"); - Serial.print(rc.radio_in[6], DEC); - Serial.print("\tch8 :"); - Serial.println(rc.radio_in[7], DEC); -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_test/APM_RC_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_test/APM_RC_test.pde deleted file mode 100644 index 33c722863b..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/APM_RC_test/APM_RC_test.pde +++ /dev/null @@ -1,59 +0,0 @@ -/* - Example of APM2_RC library. - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com - - Print Input values and send Output to the servos - (Works with last PPM_encoder firmware) -*/ - -#include // ArduPilot Mega RC Library -APM2_RC rc; - -#define CH_ROLL 0 -#define CH_PITCH 1 -#define CH_THROTTLE 2 -#define CH_RUDDER 3 - -void setup() -{ - Serial.begin(38400); - Serial.println("ArduPilot Mega RC library test"); - - //rc.set_channel_direction(CH_ROLL, -1); - //rc.set_channel_direction(CH_PITCH, -1); - //rc.set_channel_direction(CH_THROTTLE, -1); - //rc.set_channel_direction(CH_RUDDER, -1); - rc.init(); - delay(1000); -} - -void loop() -{ - delay(20); - rc.read_pwm(); - - for(int y = 0; y < 8; y++) { - rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos - } - //print_pwm(); -} - -void print_pwm() -{ - Serial.print("ch1 "); - Serial.print(rc.radio_in[CH_ROLL], DEC); - Serial.print("\tch2: "); - Serial.print(rc.radio_in[CH_PITCH], DEC); - Serial.print("\tch3 :"); - Serial.print(rc.radio_in[CH_THROTTLE], DEC); - Serial.print("\tch4 :"); - Serial.print(rc.radio_in[CH_RUDDER], DEC); - Serial.print("\tch5 "); - Serial.print(rc.radio_in[4], DEC); - Serial.print("\tch6: "); - Serial.print(rc.radio_in[5], DEC); - Serial.print("\tch7 :"); - Serial.print(rc.radio_in[6], DEC); - Serial.print("\tch8 :"); - Serial.println(rc.radio_in[7], DEC); -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde deleted file mode 100644 index 810b2df8ac..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde +++ /dev/null @@ -1,48 +0,0 @@ -/* - Example of AP_RC library. - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com - - Print Input values and send Output to the servos - (Works with last PPM_encoder firmware) -*/ - -#include // ArduPilot Mega RC Library -AP_RC rc; - -#define CH_ROLL 0 -#define CH_PITCH 1 -#define CH_THROTTLE 2 -#define CH_RUDDER 3 - -void setup() -{ - Serial.begin(38400); - Serial.println("ArduPilot RC Elevon library test"); - - rc.set_mix_mode(1); // 1 = elevons, 0 = normal - rc.init(); - - delay(1000); -} -void loop() -{ - delay(60); - rc.read_pwm(); - for(int y = 0; y < 4; y++) { - rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos - } - print_pwm(); -} - - -void print_pwm() -{ - Serial.print("ch1 "); - Serial.print(rc.radio_in[CH_ROLL], DEC); - Serial.print("\tch2: "); - Serial.print(rc.radio_in[CH_PITCH], DEC); - Serial.print("\tch3 :"); - Serial.print(rc.radio_in[CH_THROTTLE], DEC); - Serial.print("\tch4 :"); - Serial.println(rc.radio_in[CH_RUDDER], DEC); -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_test/AP_RC_test.pde b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_test/AP_RC_test.pde deleted file mode 100644 index 3b2b02c90c..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/examples/AP_RC_test/AP_RC_test.pde +++ /dev/null @@ -1,50 +0,0 @@ -/* - Example of AP_RC library. - Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com - - Print Input values and send Output to the servos - (Works with last PPM_encoder firmware) -*/ - -#include // ArduPilot RC Library -AP_RC rc; - -#define CH_ROLL 0 -#define CH_PITCH 1 -#define CH_THROTTLE 2 -#define CH_RUDDER 3 - -void setup() -{ - Serial.begin(38400); - Serial.println("ArduPilot RC library test"); - - //rc.set_channel_direction(CH_ROLL, -1); - //rc.set_channel_direction(CH_PITCH, -1); - //rc.set_channel_direction(CH_THROTTLE, -1); - //rc.set_channel_direction(CH_RUDDER, -1); - rc.init(); - delay(1000); -} - -void loop() -{ - delay(20); - rc.read_pwm(); - for(int y = 0; y < 4; y++) { - rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos - } - print_pwm(); -} - -void print_pwm() -{ - Serial.print("ch1 "); - Serial.print(rc.radio_in[CH_ROLL], DEC); - Serial.print("\tch2: "); - Serial.print(rc.radio_in[CH_PITCH], DEC); - Serial.print("\tch3 :"); - Serial.print(rc.radio_in[CH_THROTTLE], DEC); - Serial.print("\tch4 :"); - Serial.println(rc.radio_in[CH_RUDDER], DEC); -} \ No newline at end of file diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/keywords.txt b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/keywords.txt deleted file mode 100644 index 26800b80d5..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/RC/keywords.txt +++ /dev/null @@ -1,4 +0,0 @@ -AP_RC KEYWORD1 -init KEYWORD2 -set_ch_pwm KEYWORD2 -read_pwm KEYWORD2 diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.cpp b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.cpp deleted file mode 100644 index a04c6037b4..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - AP_Radio.cpp - Radio library for Arduino - Code by Jason Short. DIYDrones.com - - This library 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. - -*/ - -#include "Waypoints.h" - -Waypoints::Waypoints() -{ -} - -void -Waypoints::set_waypoint_with_index(Waypoints::WP wp, uint8_t i) -{ - i = constrain(i, 0, _total); - uint32_t mem = _start_byte + (i * _wp_size); - - eeprom_busy_wait(); - eeprom_write_byte((uint8_t *) mem, wp.id); - - mem++; - eeprom_busy_wait(); - eeprom_write_byte((uint8_t *) mem, wp.p1); - - mem++; - eeprom_busy_wait(); - eeprom_write_dword((uint32_t *) mem, wp.alt); - - mem += 4; - eeprom_busy_wait(); - eeprom_write_dword((uint32_t *) mem, wp.lat); - - mem += 4; - eeprom_busy_wait(); - eeprom_write_dword((uint32_t *) mem, wp.lng); -} - -Waypoints::WP -Waypoints::get_waypoint_with_index(uint8_t i) -{ - Waypoints::WP wp; - - i = constrain(i, 0, _total); - uint32_t mem = _start_byte + (i * _wp_size); - - eeprom_busy_wait(); - wp.id = eeprom_read_byte((uint8_t *)mem); - - mem++; - eeprom_busy_wait(); - wp.p1 = eeprom_read_byte((uint8_t *)mem); - - mem++; - eeprom_busy_wait(); - wp.alt = (long)eeprom_read_dword((uint32_t *)mem); - - mem += 4; - eeprom_busy_wait(); - wp.lat = (long)eeprom_read_dword((uint32_t *)mem); - - mem += 4; - eeprom_busy_wait(); - wp.lng = (long)eeprom_read_dword((uint32_t *)mem); -} - - -Waypoints::WP -Waypoints::get_current_waypoint(void) -{ - return get_waypoint_with_index(_index); -} - -uint8_t -Waypoints::get_index(void) -{ - return _index; -} - -void -Waypoints::next_index(void) -{ - _index++; - if (_index >= _total) - _index == 0; -} - -void -Waypoints::set_index(uint8_t i) -{ - i = constrain(i, 0, _total); -} - -uint8_t -Waypoints::get_total(void) -{ - return _total; -} -void -Waypoints::set_total(uint8_t total) -{ - _total = total; -} - -void -Waypoints::set_start_byte(uint16_t start_byte) -{ - _start_byte = start_byte; -} - -void -Waypoints::set_wp_size(uint8_t wp_size) -{ - _wp_size = wp_size; -} diff --git a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.h b/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.h deleted file mode 100644 index ddf03378a4..0000000000 --- a/Tools/ArduPPM/WorkBasket/Jeti_Duplex/X-DIY_Jeti_V2/libraries/Waypoints/Waypoints.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef Waypoints_h -#define Waypoints_h - -#include -#include "WProgram.h" -#include - -class Waypoints -{ - public: - Waypoints(); - - struct WP { - uint8_t id; // for commands - int8_t p1; // for commands - int32_t alt; // Altitude in centimeters (meters * 100) - int32_t lat; // Lattitude * 10**7 - int32_t lng; // Longitude * 10**7 - }; - - WP get_waypoint_with_index(uint8_t i); - WP get_current_waypoint(void); - - void set_waypoint_with_index(Waypoints::WP wp, uint8_t i); - - void set_start_byte(uint16_t start_byte); - void set_wp_size(uint8_t wp_size); - - void next_index(void); - uint8_t get_index(void); - void set_index(uint8_t i); - - uint8_t get_total(void); - void set_total(uint8_t total); - - - - private: - uint16_t _start_byte; - uint8_t _wp_size; - uint8_t _index; - uint8_t _total; -}; - -#endif -