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

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Jason Short 2011-10-01 15:55:31 -07:00
parent 0bfa67f413 dcef0f34e9
commit 1c284cacb3
166 changed files with 24805 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/ArduPlane.pde
View File

@ -215,10 +215,12 @@ static const char* flight_mode_strings[] = {
2 Elevator 2 Elevator
3 Throttle 3 Throttle
4 Rudder (if we have ailerons) 4 Rudder (if we have ailerons)
5 Mode 5 Aux5
6 TBD 6 Aux6
7 TBD 7 Aux7
8 TBD 8 Aux8/Mode
Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
See libraries/RC_Channel/RC_Channel_aux.h for more information
*/ */
// Failsafe // Failsafe

2
Tools/ArduPPM/ATMega328p/Encoder-PPM.c
View File

@ -37,7 +37,7 @@
// PREPROCESSOR DIRECTIVES // PREPROCESSOR DIRECTIVES
// ------------------------------------------------------------------------------------------------------------------------------------------------------------ // ------------------------------------------------------------------------------------------------------------------------------------------------------------
#include "ppm_encoder.h" #include "..\Libraries\ppm_encoder.h"
#include <util/delay.h> #include <util/delay.h>

2
Tools/ArduPPM/Libraries/readme.txt
View File

@ -1,2 +1,2 @@
libraries used by all code bases libraries used by all ArduPPM code bases

166
Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.cpp Normal file
View File

@ -0,0 +1,166 @@
/*
JetiBox.cpp, Version 1.0 beta
July 2010, by Uwe Gartmann
Library acts as a Sensor when connected to a Jeti Receiver
written for Arduino Mega / ArduPilot Mega
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 <avr/io.h>
#include <avr/interrupt.h>
#include "wiring.h"
#include "mySerial0.h"
#include "JetiBox.h"
#define LCDLine1 1
#define LCDLine2 17
#define LCDMaxChar 32
//define Jeti-Box Display
struct jeti_box {
unsigned char lcd[32];
volatile unsigned char lcdpos;
volatile unsigned char keys;
volatile unsigned char sendpos;
};
// create jbox
jeti_box jb = {{0},0,0,0};
unsigned char dummy;
unsigned char lastkey = 0;
ISR(USART3_RX_vect) //serial data available
{
// save response data to keys
dummy = UDR3;
if (dummy != 0xFF)
{
jb.keys = dummy;
// disable this interrupt
UCSR3B &= ~(1<<RXEN3);
}
}
ISR(USART3_UDRE_vect) //transmit buffer empty
/*
* jbox.sendpos = 0 -> interrupt is enabled (function start() ), send startbyte with 9.bit=0
* jbox.sendpos = 1-32 -> send display data with 9.bit=1
* jbox.sendpos = 33 -> send endbyte with 9.bit=0
* jbox.sendpos = 34 -> reset sendpos=0 -> disable this interrupt
*/
{
switch (jb.sendpos)
{
case 0:
// send start byte with 9.bit=0
UCSR3B &= ~(1<<TXB83); // clear bit 9
UDR3 = 0xFE;
jb.sendpos++;
break;
case 33:
// send end byte with 9.bit=0
UCSR3B &= ~(1<<TXB83);
UDR3 = 0xFF;
jb.sendpos++;
break;
case 34:
// disable interrupt transmit buffer empty
UCSR3B &= ~(1<<UDRIE3);
// set sendpos -> 0
jb.sendpos = 0;
// enable receiver interrupt for reading key byte
UCSR3B |= (1<<RXEN3);
break;
default:
// set 9.bit=1
UCSR3B |= (1<<TXB83);
// send byte from LCD buffer
UDR3 = jb.lcd[jb.sendpos];
// increment to next byte
jb.sendpos++;
}
}
JetiBox::JetiBox()
{
// Class constructor
}
// Public Methods --------------------------------------------------------------------
void JetiBox::begin()
{
#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 baudrate
UCSR3A = (1<<U2X3); //U2X0 enabled!
UBRR3H=(_UBRR3>>8); //high byte
UBRR3L=_UBRR3; //low byte
// Set frame format: 9data, odd parity, 2stop bit
UCSR3C = (0<<UMSEL30)|(3<<UPM30)|(1<<USBS3)|(3<<UCSZ30);
// Enable receiver and transmitter, set frame size
UCSR3B = (1<<TXEN3)|(1<<RXCIE3)|(1<<UCSZ32);
}
void JetiBox::refresh()
{
UCSR3B |= (1<<UDRIE3); // Enable Interrupt
}
uint8_t JetiBox::keys(void)
{
unsigned char c = (jb.keys>>4) xor 0x0F;
if (lastkey==c)
{
return 0;
}else{
return lastkey = c;
}
}
void JetiBox::write(uint8_t c)
{
jb.lcd[jb.lcdpos] = c;
if (jb.lcdpos < LCDMaxChar)
{
jb.lcdpos++;
}
}
void JetiBox::line1()
{
jb.lcdpos = LCDLine1;
}
void JetiBox::line2()
{
jb.lcdpos = LCDLine2;
}
// Preinstantiate Objects //////////////////////////////////////////////////////
JetiBox JBox;

51
Tools/ArduPPM/WorkBasket/Jeti Duplex/Jetibox/JetiBox.h Normal file
View File

@ -0,0 +1,51 @@
/*
JetiBox.h, Version 1.0 beta
July 2010, by Uwe Gartmann
Library acts as a Sensor when connected to a Jeti Receiver
written for Arduino Mega / ArduPilot Mega
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
*/
#ifndef JetiBox_h
#define JetiBox_h
#include <inttypes.h>
#include "Print.h"
#define jbox_key_up 0b0010
#define jbox_key_right 0b0001
#define jbox_key_down 0b0100
#define jbox_key_left 0b1000
struct jeti_box;
class JetiBox : public Print
{
public:
JetiBox();
void begin();
void refresh();
uint8_t keys(void);
virtual void write(uint8_t);
using Print::write; // pull in write(str) and write(buf, size) from Print
void line1();
void line2();
};
extern JetiBox JBox;
#endif

925
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h Normal file
View File

@ -0,0 +1,925 @@
//
// 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.
//

914
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config.h.reference Normal file
View File

@ -0,0 +1,914 @@
//
// 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.

19
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/APM_Config_xplane.h Normal file
View File

@ -0,0 +1,19 @@
#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

845
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/ArduPilotMega.pde Normal file
View File

@ -0,0 +1,845 @@
// -*- 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 <FastSerial.h>
#include <JETI_Box.h>
#include <AP_Common.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <APM_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h>
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <APM_Compass.h> // ArduPilot Mega Magnetometer Library
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// 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
}

343
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Attitude.pde Normal file
View File

@ -0,0 +1,343 @@
//****************************************************************
// 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;
}

387
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/DCM.pde Normal file
View File

@ -0,0 +1,387 @@
// 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];
}
}
}

389
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM map.txt Normal file
View File

@ -0,0 +1,389 @@
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 ..

286
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/EEPROM.pde Normal file
View File

@ -0,0 +1,286 @@
// -*- 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));
}

176
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Ardupilot.pde Normal file
View File

@ -0,0 +1,176 @@
// -*- 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

1258
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_DebugTerminal.pde Normal file
View File

File diff suppressed because it is too large Load Diff

192
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_IMU_ouput.pde Normal file
View File

@ -0,0 +1,192 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_IMU
// 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 id)
{
send_message(id,0l);
}
void send_message(byte id, long param)
{
switch(id) {
case MSG_ATTITUDE:
print_attitude();
break;
case MSG_LOCATION: // ** Location/GPS message
print_location();
break;
}
}
void send_message(byte severity, const char *str)
{
if(severity >= 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

238
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Jason_text.pde Normal file
View File

@ -0,0 +1,238 @@
// -*- 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<wp_total; i++){
cmd = get_wp_with_index(i);
print_waypoint(&cmd, i);
}
}
#endif

371
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Standard.pde Normal file
View File

@ -0,0 +1,371 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Doug _ command index is a byte and not an int
#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD
#if GCS_PORT == 3
# define SendSer Serial3.print
#else
# define SendSer Serial.print
#endif
// The functions included in this file are for use with the standard binary communication protocol and standard Ground Control Station
void acknowledge(byte id, byte check1, byte check2) {
byte mess_buffer[6];
byte mess_ck_a = 0;
byte mess_ck_b = 0;
int ck;
SendSer("4D"); // This is the message preamble
mess_buffer[0] = 0x03;
ck = 3;
mess_buffer[1] = 0x00; // Message ID
mess_buffer[2] = 0x01; // Message version
mess_buffer[3] = id;
mess_buffer[4] = check1;
mess_buffer[5] = check2;
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 send_message(byte id) {
send_message(id, 0l);
}
void send_message(byte id, long param) {
byte mess_buffer[54];
byte mess_ck_a = 0;
byte mess_ck_b = 0;
int tempint;
int ck;
long templong;
SendSer("4D"); // This is the message preamble
switch(id) {
case MSG_HEARTBEAT: // ** System Status message
mess_buffer[0] = 0x07;
ck = 7;
mess_buffer[3] = control_mode; // Mode
templong = millis() / 1000; // Timestamp - Seconds since power - up
mess_buffer[4] = templong & 0xff;
mess_buffer[5] = (templong >> 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<ck+2;i++) mess_buffer[i] = status_message[i-3];
// break;
case MSG_PERF_REPORT: // ** Performance Monitoring message
mess_buffer[0] = 0x10;
ck = 16;
templong = millis() - perf_mon_timer; // Report interval (milliseconds) 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;
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

9
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_XDIY.pde Normal file
View File

@ -0,0 +1,9 @@
#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

127
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/GCS_Xplane.pde Normal file
View File

@ -0,0 +1,127 @@
// -*- 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<wp_total; i++){
cmd = get_wp_with_index(i);
print_waypoint(&cmd, i);
}
}
#endif

78
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/HIL_output.pde Normal file
View File

@ -0,0 +1,78 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if ENABLE_HIL
// An Xplane/Flightgear output
// Use with the GPS_IMU to do Hardware in teh loop simulations
byte buf_len = 0;
byte out_buffer[32];
void output_HIL(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(servo_out[CH_ROLL])); // 0 bytes 0, 1
output_int((int)(servo_out[CH_PITCH])); // 1 bytes 2, 3
output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4, 5
output_int((int)(servo_out[CH_RUDDER])); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)next_WP.alt / 100); // 6 bytes 12, 13
output_int((int)energy_error); // 7 bytes 14,15
output_byte(wp_index); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17
// print out the buffer and checksum
// ---------------------------------
print_buffer();
}
// This is for debugging only!,
// I just move the underscore to keep the above version pristene.
void output_HIL_(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(servo_out[CH_ROLL])); // 0 bytes 0, 1
output_int((int)(servo_out[CH_PITCH])); // 1 bytes 2, 3
output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4, 5
output_int((int)(servo_out[CH_RUDDER])); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8, 9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)roll_sensor); // 6 bytes 12,13
output_int((int)loiter_total); // 7 bytes 14,15
output_byte(wp_index); // 8 bytes 16
output_byte(control_mode); // 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;
}
#endif

264
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Jeti.pde Normal file
View File

@ -0,0 +1,264 @@
// 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<LCDMaxPos; i++) {
if (i<length) {
statustxt[i] = str[i];
} else {
statustxt[i] = 32; // if text < 32 chars -> 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;
}
}

596
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/Log.pde Normal file
View File

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

75
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/command description.txt Normal file
View File

@ -0,0 +1,75 @@
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

228
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands.pde Normal file
View File

@ -0,0 +1,228 @@
// -*- 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 <increment_WP_index> WP index is incremented to ");
SendDebugln(wp_index,DEC);
}else{
SendDebug("MSG <increment_WP_index> 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 <set_next_wp> 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(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
print_current_waypoints();
target_bearing = get_bearing(&current_loc, &next_WP);
old_target_bearing = target_bearing;
// this is used to offset the shrinking longitude as we go towards the poles
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
// run this at setup on the ground
// -------------------------------
void init_home()
{
SendDebugln("MSG: <init_home> init home");
// Extra read just in case
// -----------------------
//GPS.Read();
// block until we get a good fix
// -----------------------------
while (!GPS.new_data || !GPS.fix) {
GPS.update();
}
home.id = CMD_WAYPOINT;
home.lng = GPS.longitude; // Lon * 10**7
home.lat = GPS.latitude; // Lat * 10**7
home.alt = GPS.altitude;
home_is_set = TRUE;
// ground altitude in centimeters for pressure alt calculations
// ------------------------------------------------------------
ground_alt = GPS.altitude;
press_alt = GPS.altitude; // Set initial value for filter
save_pressure_data();
// Save Home to EEPROM
// -------------------
set_wp_with_index(home, 0);
// Save prev loc
// -------------
prev_WP = home;
// 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();
}

480
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/commands_process.pde Normal file
View File

@ -0,0 +1,480 @@
// 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 <load_next_command> 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 <load_next_command> 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 <load_next_command> Preload RTL cmd id: ");
//SendDebugln(next_command.id,DEC);
break;
}
}
}
void process_next_command()
{
// these are waypoint/Must commands
// ---------------------------------
if (command_must_index == 0){ // no current command loaded
if (next_command.id >= 0x10 && next_command.id <= 0x1F ){
increment_WP_index();
save_command_index(); // to Recover from in air Restart
command_must_index = wp_index;
//SendDebug("MSG <process_next_command> 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: <process_must>");
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,"<process_may> 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, "<process_now> 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 <verify_must: CMD_WAYPOINT> REACHED_WAYPOINT #");
SendDebugln(command_must_index,DEC);
// clear the command queue;
command_must_index = 0;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
send_message(SEVERITY_MEDIUM,"<verify_must: CMD_WAYPOINT> 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,"<verify_must: CMD_LOITER_N_TURNS> 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,"<verify_must: CMD_LOITER_TIME> 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,"<verify_must: default> No current Must commands");
break;
}
}
void verify_may()
{
switch(command_may_ID) {
case CMD_DELAY:
if ((millis() - delay_start) > delay_timeout){
command_may_index = 0;
delay_timeout = 0;
}
case CMD_LAND_OPTIONS:
if ((wp_distance > 0) && (wp_distance <= landing_distance)) {
//Serial.print("XXX REACHED_WAYPOINT #");
//Serial.println(command_must_index,DEC);
// clear the command queue;
command_may_index = 0;
}
break;
}
}

569
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/config.h Normal file
View File

@ -0,0 +1,569 @@
// -*- 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

58
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/control_modes.pde Normal file
View File

@ -0,0 +1,58 @@
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;
}
}

70
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/debug.pde Normal file
View File

@ -0,0 +1,70 @@
#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

345
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/defines.h Normal file
View File

@ -0,0 +1,345 @@
// -*- 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)

226
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/events.pde Normal file
View File

@ -0,0 +1,226 @@
/*
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;
}
}

230
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/navigation.pde Normal file
View File

@ -0,0 +1,230 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//****************************************************************
// Function that will calculate the desired direction to fly and altitude error
//****************************************************************
void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (GPS.fix == 0)
{
GPS.new_data = false;
return;
}
if(next_WP.lat == 0){
return;
}
// We only perform most nav computations if we have new gps data to work with
// --------------------------------------------------------------------------
if(GPS.new_data){
GPS.new_data = false;
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrac correction
// ------------------------------------------
nav_bearing = target_bearing;
// waypoint distance from plane
// ----------------------------
wp_distance = getDistance(&current_loc, &next_WP);
if (wp_distance < 0){
send_message(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return;
}
// check if we have missed the WP
loiter_delta = (target_bearing - old_target_bearing)/100;
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta);
calc_bearing_error();
// control mode specific updates to nav_bearing
update_navigation();
}
}
void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
// distance and bearing calcs only
if(control_mode == AUTO){
verify_must();
verify_may();
}else{
switch(control_mode){
case LOITER:
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(&current_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;
}

180
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/radio.pde Normal file
View File

@ -0,0 +1,180 @@
//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

65
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/sensors.pde Normal file
View File

@ -0,0 +1,65 @@
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;
}

410
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/setup.pde Normal file
View File

@ -0,0 +1,410 @@
// -*- 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;
}
}

491
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/system.pde Normal file
View File

@ -0,0 +1,491 @@
// -*- 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,"<init_ardupilot> 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,"<startup_ground> GROUND START");
#if(GROUND_START_DELAY > 0)
send_message(SEVERITY_LOW,"<startup_ground> 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,"<startup_ground> zero airspeed calibrated");
#else
send_message(SEVERITY_LOW,"<startup_ground> 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("<startup_IMU_ground> 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("<startup_IMU_ground> 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(" <startup_IMU_ground> 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;
}

433
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/X-DIY/test.pde Normal file
View File

@ -0,0 +1,433 @@
// 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"));
}

149
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.cpp Normal file
View File

@ -0,0 +1,149 @@
/*
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 <inttypes.h>
#include <avr/interrupt.h>
#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<<UDRE2)) );
/* Put data into buffer, sends the data */
UDR2 = data;
/* Wait for data to be received */
while ( !(UCSR2A & (1<<RXC2)) );
/* Get and return received data from buffer */
return UDR2;
}
ISR (TIMER2_OVF_vect)
{
uint8_t ch;
unsigned int adc_tmp;
//bit_set(PORTL,6); // To test performance
bit_clear(PORTC,4); // Enable Chip Select (PIN PC4)
ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel
for (ch=0;ch<8;ch++)
{
if (adc_counter[ch] >= 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<<PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
// Set MSPI mode of operation and SPI data mode 0.
UCSR2C = (1<<UMSEL21)|(1<<UMSEL20); //|(0<<UCPHA2)|(0<<UCPOL2);
// Enable receiver and transmitter.
UCSR2B = (1<<RXEN2)|(1<<TXEN2);
// Set Baud rate
UBRR2 = 2; // SPI clock running at 2.6MHz
// Enable Timer2 Overflow interrupt to capture ADC data
TIMSK2 = 0; // Disable interrupts
TCCR2A = 0; // normal counting mode
TCCR2B = _BV(CS21)|_BV(CS22); // Set prescaler of 256
TCNT2 = 0;
TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
}
// Read one channel value
int APM_ADC_Class::Ch(unsigned char ch_num)
{
int result;
cli(); // We stop interrupts to read the variables
if (adc_counter[ch_num]>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;

24
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/APM_ADC.h Normal file
View File

@ -0,0 +1,24 @@
#ifndef APM_ADC_h
#define APM_ADC_h
#define bit_set(p,m) ((p) |= ( 1<<m))
#define bit_clear(p,m) ((p) &= ~(1<<m))
// We use Serial Port 2 in SPI Mode
#define ADC_DATAOUT 51 // MOSI
#define ADC_DATAIN 50 // MISO
#define ADC_SPICLOCK 52 // SCK
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
class APM_ADC_Class
{
private:
public:
APM_ADC_Class(); // Constructor
void Init();
int Ch(unsigned char ch_num);
};
extern APM_ADC_Class APM_ADC;
#endif

33
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/examples/APM_ADC_test/APM_ADC_test.pde Normal file
View File

@ -0,0 +1,33 @@
/*
Example of APM_ADC library.
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
*/
#include <APM_ADC.h> // 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();
}
}

3
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_ADC/keywords.txt Normal file
View File

@ -0,0 +1,3 @@
APM_ADC KEYWORD1
Init KEYWORD2
Ch KEYWORD2

243
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.cpp Normal file
View File

@ -0,0 +1,243 @@
/*
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 <inttypes.h>
#include <avr/interrupt.h>
#include "WConstants.h"
}
#include <Wire.h>
#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)<<oss + 2) >> 2; // BAD
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0
tmp = ac1;
tmp = (tmp*4 + x3)<<oss;
b3 = (tmp+2)/4;
x1 = ac3 * b6 >> 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;

34
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/APM_BMP085.h Normal file
View File

@ -0,0 +1,34 @@
#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

41
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde Normal file
View File

@ -0,0 +1,41 @@
/*
Example of APM_BMP085 (absolute pressure sensor) library.
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
*/
#include <Wire.h>
#include <APM_BMP085.h> // 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();
}
}

5
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BMP085/keywords.txt Normal file
View File

@ -0,0 +1,5 @@
APM_BMP085 KEYWORD1
Init KEYWORD2
Read KEYWORD2
Press KEYWORD2
Temp KEYWORD2

206
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.cpp Normal file
View File

@ -0,0 +1,206 @@
// -*- 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;
}
}

267
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/APM_BinComm.h Normal file
View File

@ -0,0 +1,267 @@
// -*- 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 <string.h>
#include <inttypes.h>
#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

6
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/keywords.txt Normal file
View File

@ -0,0 +1,6 @@
APM_BinComm KEYWORD1
update KEYWORD2
messagesReceived KEYWORD2
badMessagesReceived KEYWORD2
messagesSent KEYWORD2

224
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.def Normal file
View File

@ -0,0 +1,224 @@
#
# 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 <MESSAGE_ID> <MESSAGE_NAME>
#
# <MESSAGE_NAME> is a valid member of BinComm::MessageID.
# <MESSAGE_ID> is the message ID byte
#
# Following a message declaration the fields of the message are
# defined in the format:
#
# <TYPE> <NAME> [<COUNT>]
#
# <TYPE> is a C type corresponding to the field. The type must be a single
# word, either an integer from <inttypes.h> or "char".
# <NAME> is the name of the field; it should be unique within the message
# <COUNT> 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

1301
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protocol.h Normal file
View File

File diff suppressed because it is too large Load Diff

174
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/protocol/protogen.awk Normal file
View File

@ -0,0 +1,174 @@
#
# 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 <inttypes.h> 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++
}

14
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/Makefile Normal file
View File

@ -0,0 +1,14 @@
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)

15
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/WProgram.h Normal file
View File

@ -0,0 +1,15 @@
#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

110
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_BinComm/test/test.cpp Normal file
View File

@ -0,0 +1,110 @@
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
// test harness for the APM_BinComm bits
#include <stdint.h>
#include <err.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#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 <port>");
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();
}
}

235
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.cpp Normal file
View File

@ -0,0 +1,235 @@
// -*- 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 <math.h>
#include "WConstants.h"
}
#include <Wire.h>
#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;

69
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/APM_Compass.h Normal file
View File

@ -0,0 +1,69 @@
#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

87
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde Normal file
View File

@ -0,0 +1,87 @@
/*
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 <Wire.h>
#include <APM_Compass.h> // Compass Library
#include <AP_Math.h> // 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();
}
}

19
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_Compass/keywords.txt Normal file
View File

@ -0,0 +1,19 @@
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

150
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.cpp Normal file
View File

@ -0,0 +1,150 @@
/*
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 <inttypes.h>
#include <avr/interrupt.h>
#include <avr/io.h>
#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

25
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/APM_FastSerial.h Normal file
View File

@ -0,0 +1,25 @@
#ifndef APM_FastSerial_h
#define APM_FastSerial_h
#include <inttypes.h>
#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

91
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde Normal file
View File

@ -0,0 +1,91 @@
/*
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 <APM_FastSerial.h> // 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++;
}
}

2
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_FastSerial/keywords.txt Normal file
View File

@ -0,0 +1,2 @@
APM_FastSerial KEYWORD1
APM_FastSerial3 KEYWORD1

193
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.cpp Normal file
View File

@ -0,0 +1,193 @@
/*
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 <avr/interrupt.h>
#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 (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
if (Pulse_Width>8000) // 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<<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
pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
pinMode(4,OUTPUT); // (PE3/OC3A)
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
pinMode(44,OUTPUT); //OUT1 (PL5/OC5C)
pinMode(45,OUTPUT); //OUT0 (PL4/OC5B)
pinMode(46,OUTPUT); // (PL3/OC5A)
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
pinMode(49, INPUT); // ICP4 pin (PL0) (PPM input)
pinMode(7,OUTPUT); //OUT5 (PH4/OC4B)
pinMode(8,OUTPUT); //OUT4 (PH5/OC4C)
TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));
//Prescaler set to 8, that give us a resolution of 0.5us
// Input Capture rising edge
TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
OCR4A = 40000; ///50hz freq.
OCR4B = 3000; //PH4, OUT5
OCR4C = 3000; //PH5, OUT4
//TCCR4B |=(1<<ICES4); //Changing edge detector (rising edge).
//TCCR4B &=(~(1<<ICES4)); //Changing edge detector. (falling edge)
TIMSK4 |= (1<<ICIE4); // Enable Input Capture interrupt. Timer interrupt mask
}
void APM_RC_Class::OutputCh(unsigned char ch, int pwm)
{
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
pwm<<=1; // pwm*2;
switch(ch)
{
case 0: OCR5B=pwm; break; //ch0
case 1: OCR5C=pwm; break; //ch1
case 2: OCR1B=pwm; break; //ch2
case 3: OCR1C=pwm; break; //ch3
case 4: OCR4C=pwm; break; //ch4
case 5: OCR4B=pwm; break; //ch5
case 6: OCR3C=pwm; break; //ch6
case 7: OCR3B=pwm; break; //ch7
case 8: OCR5A=pwm; break; //ch8, PL3
case 9: OCR1A=pwm; break; //ch9, PB5
case 10: OCR3A=pwm; break; //ch10, PE3
}
}
int APM_RC_Class::InputCh(unsigned char ch)
{
int result;
int result2;
// Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
// We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
result = PWM_RAW[ch]>>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)

24
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/APM_RC.h Normal file
View File

@ -0,0 +1,24 @@
#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

31
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/examples/APM_radio/APM_radio.pde Normal file
View File

@ -0,0 +1,31 @@
/*
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 <APM_RC.h> // 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();
}
}

8
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC/keywords.txt Normal file
View File

@ -0,0 +1,8 @@
APM_RC KEYWORD1
begin KEYWORD2
InputCh KEYWORD2
OutputCh KEYWORD2
GetState KEYWORD2
Force_Out0_Out1 KEYWORD2
Force_Out2_Out3 KEYWORD2
Force_Out6_Out7 KEYWORD2

169
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.cpp Normal file
View File

@ -0,0 +1,169 @@
/*
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 <avr/interrupt.h>
#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 (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
if (Pulse_Width>8000) // 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<<WGM11)|(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 = 6600; //300hz freq...
// Init PWM Timer 3
pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
//pinMode(4,OUTPUT); // (PE3/OC3A)
TCCR3A =((1<<WGM31)|(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 (standard servos)
// Init PWM Timer 5
pinMode(44,OUTPUT); //OUT1 (PL5/OC5C)
pinMode(45,OUTPUT); //OUT0 (PL4/OC5B)
//pinMode(46,OUTPUT); // (PL3/OC5A)
TCCR5A =((1<<WGM51)|(1<<COM5B1)|(1<<COM5C1));
TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
//OCR5A = 3000; //PL3,
OCR5B = 3000; //PL4, OUT0
OCR5C = 3000; //PL5, OUT1
ICR5 = 6600; //300hz freq
// 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)
TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));
//Prescaler set to 8, that give us a resolution of 0.5us
// Input Capture rising edge
TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
OCR4A = 40000; ///50hz freq. (standard servos)
OCR4B = 3000; //PH4, OUT5
OCR4C = 3000; //PH5, OUT4
//TCCR4B |=(1<<ICES4); //Changing edge detector (rising edge).
//TCCR4B &=(~(1<<ICES4)); //Changing edge detector. (falling edge)
TIMSK4 |= (1<<ICIE4); // Enable Input Capture interrupt. Timer interrupt mask
}
void APM_RC_QUAD_Class::OutputCh(unsigned char ch, int pwm)
{
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
pwm<<=1; // pwm*2;
switch(ch)
{
case 0: OCR5B=pwm; break; //ch0
case 1: OCR5C=pwm; break; //ch1
case 2: OCR1B=pwm; break; //ch2
case 3: OCR1C=pwm; break; //ch3
case 4: OCR4C=pwm; break; //ch4
case 5: OCR4B=pwm; break; //ch5
case 6: OCR3C=pwm; break; //ch6
case 7: OCR3B=pwm; break; //ch7
}
}
int APM_RC_QUAD_Class::InputCh(unsigned char ch)
{
int result;
int result2;
// Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
// We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
result = PWM_RAW[ch]>>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;

21
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/APM_RC_QUAD.h Normal file
View File

@ -0,0 +1,21 @@
#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

31
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/examples/APM_radio_quad/APM_radio_quad.pde Normal file
View File

@ -0,0 +1,31 @@
/*
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 <APM_RC.h> // 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();
}
}

5
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/APM_RC_QUAD/keywords.txt Normal file
View File

@ -0,0 +1,5 @@
APM_RC_QUAD KEYWORD1
begin KEYWORD2
InputCh KEYWORD2
OutputCh KEYWORD2
GetState KEYWORD2

17
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.cpp Normal file
View File

@ -0,0 +1,17 @@
// -*- 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"

53
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/AP_Common.h Normal file
View File

@ -0,0 +1,53 @@
// -*- 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 <stdint.h>
#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

21
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/c++.cpp Normal file
View File

@ -0,0 +1,21 @@
// -*- 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 <stdlib.h>
void * operator new(size_t size)
{
return(calloc(size, 1));
}
void operator delete(void *p)
{
if (p)
free(p);
}

45
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/examples/menu/menu.pde Normal file
View File

@ -0,0 +1,45 @@
#include <FastSerial.h>
#include <AP_Common.h>
#include <avr/pgmspace.h>
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)
{
}

138
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/include/menu.h Normal file
View File

@ -0,0 +1,138 @@
// -*- 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)

4
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/keywords.txt Normal file
View File

@ -0,0 +1,4 @@
Menu KEYWORD1
run KEYWORD2
Location KEYWORD2

129
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Common/menu.cpp Normal file
View File

@ -0,0 +1,129 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// Simple commandline menu system.
//
#include <FastSerial.h>
#include <ctype.h>
#include <string.h>
#include <avr/pgmspace.h>
#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]));
}

115
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.cpp Normal file
View File

@ -0,0 +1,115 @@
/*
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 <math.h>
#include "WConstants.h"
}
#include <Wire.h>
#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);
}

17
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/AP_Compass.h Normal file
View File

@ -0,0 +1,17 @@
#ifndef AP_Compass_h
#define AP_Compass_h
#include <Compass.h>
class AP_Compass : public Compass
{
public:
AP_Compass(); // Constructor
void init();
void update();
void calculate(float roll, float pitch);
private:
};
#endif

23
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/Compass.h Normal file
View File

@ -0,0 +1,23 @@
#ifndef Compass_h
#define Compass_h
#include <inttypes.h>
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

40
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde Normal file
View File

@ -0,0 +1,40 @@
/*
Example of APM_Compass library (HMC5843 sensor).
Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
*/
#include <Wire.h>
#include <AP_Compass.h> // 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(" )");
}
}

15
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_Compass/keywords.txt Normal file
View File

@ -0,0 +1,15 @@
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

13
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS.h Normal file
View File

@ -0,0 +1,13 @@
// -*- 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"

81
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.cpp Normal file
View File

@ -0,0 +1,81 @@
// -*- 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);
}

32
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_406.h Normal file
View File

@ -0,0 +1,32 @@
// -*- 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

162
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.cpp Normal file
View File

@ -0,0 +1,162 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// Auto-detecting pseudo-GPS driver
//
#include "AP_GPS.h"
#include <stdlib.h>
#include <stdio.h>
#include <wiring.h>
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());
}

52
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_Auto.h Normal file
View File

@ -0,0 +1,52 @@
// -*- 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 <GPS.h>
#include <FastSerial.h>
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

224
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.cpp Normal file
View File

@ -0,0 +1,224 @@
// -*- 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;i<numc;i++){ // Process bytes received
data = _port->read();
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;
}

44
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_IMU.h Normal file
View File

@ -0,0 +1,44 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_IMU_h
#define AP_GPS_IMU_h
#include <GPS.h>
#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

146
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.cpp Normal file
View File

@ -0,0 +1,146 @@
// -*- 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;
}

87
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_MTK.h Normal file
View File

@ -0,0 +1,87 @@
// -*- 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 <GPS.h>
#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

246
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.cpp Normal file
View File

@ -0,0 +1,246 @@
// -*- 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;
}

60
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_NMEA.h Normal file
View File

@ -0,0 +1,60 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_NMEA_h
#define AP_GPS_NMEA_h
#include <GPS.h>
#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

13
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_None.h Normal file
View File

@ -0,0 +1,13 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_None_h
#define AP_GPS_None_h
#include <GPS.h>
class AP_GPS_None : public GPS
{
virtual void init(void) {};
virtual void update(void) {};
};
#endif

193
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.cpp Normal file
View File

@ -0,0 +1,193 @@
// -*- 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;
}

96
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_SIRF.h Normal file
View File

@ -0,0 +1,96 @@
// -*- 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 <GPS.h>
#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

192
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.cpp Normal file
View File

@ -0,0 +1,192 @@
// -*- 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;
}

124
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/AP_GPS_UBLOX.h Normal file
View File

@ -0,0 +1,124 @@
// -*- 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 <GPS.h>
#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

35
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.cpp Normal file
View File

@ -0,0 +1,35 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include "GPS.h"
#include "WProgram.h"
#include <stdio.h>
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);
}
}

150
Tools/ArduPPM/WorkBasket/Jeti Duplex/X-DIY_Jeti_V2/libraries/AP_GPS/GPS.h Normal file
View File

@ -0,0 +1,150 @@
// -*- 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 <inttypes.h>
#include <Stream.h>
/// @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

Some files were not shown because too many files have changed in this diff Show More