mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
eba281e3de
|
@ -215,10 +215,12 @@ static const char* flight_mode_strings[] = {
|
|||
2 Elevator
|
||||
3 Throttle
|
||||
4 Rudder (if we have ailerons)
|
||||
5 Mode
|
||||
6 TBD
|
||||
7 TBD
|
||||
8 TBD
|
||||
5 Aux5
|
||||
6 Aux6
|
||||
7 Aux7
|
||||
8 Aux8/Mode
|
||||
Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
|
||||
See libraries/RC_Channel/RC_Channel_aux.h for more information
|
||||
*/
|
||||
|
||||
// Failsafe
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
// PREPROCESSOR DIRECTIVES
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
#include "ppm_encoder.h"
|
||||
#include "..\Libraries\ppm_encoder.h"
|
||||
#include <util/delay.h>
|
||||
|
||||
|
||||
|
|
|
@ -1,2 +1,2 @@
|
|||
|
||||
libraries used by all code bases
|
||||
libraries used by all ArduPPM code bases
|
|
@ -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;
|
||||
|
||||
|
||||
|
|
@ -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
|
|
@ -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.
|
||||
//
|
||||
|
|
@ -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.
|
|
@ -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
|
||||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
|
@ -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];
|
||||
}
|
||||
}
|
||||
}
|
|
@ -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 ..
|
||||
|
||||
|
||||
|
|
@ -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));
|
||||
}
|
||||
|
|
@ -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
|
File diff suppressed because it is too large
Load Diff
|
@ -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
|
|
@ -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
|
||||
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
|
||||
print_current_waypoints();
|
||||
|
||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||
old_target_bearing = target_bearing;
|
||||
// this is used to offset the shrinking longitude as we go towards the poles
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
}
|
||||
|
||||
|
||||
// run this at setup on the ground
|
||||
// -------------------------------
|
||||
void init_home()
|
||||
{
|
||||
SendDebugln("MSG: <init_home> 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();
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
|
@ -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;
|
||||
}
|
||||
}
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
|
@ -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(¤t_loc, &next_WP);
|
||||
|
||||
// nav_bearing will includes xtrac correction
|
||||
// ------------------------------------------
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
// waypoint distance from plane
|
||||
// ----------------------------
|
||||
wp_distance = getDistance(¤t_loc, &next_WP);
|
||||
|
||||
if (wp_distance < 0){
|
||||
send_message(SEVERITY_HIGH,"<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(¤t_loc, &next_WP); // Used for track following
|
||||
}
|
||||
|
||||
int get_altitude_above_home(void)
|
||||
{
|
||||
// This is the altitude above the home location
|
||||
// The GPS gives us altitude at Sea Level
|
||||
// if you slope soar, you should see a negative number sometimes
|
||||
// -------------------------------------------------------------
|
||||
return current_loc.alt - home.alt;
|
||||
}
|
||||
|
||||
long getDistance(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
if(loc1->lat == 0 || loc1->lng == 0)
|
||||
return -1;
|
||||
if(loc2->lat == 0 || loc2->lng == 0)
|
||||
return -1;
|
||||
float dlat = (float)(loc2->lat - loc1->lat);
|
||||
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
|
||||
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
|
||||
}
|
||||
|
||||
long get_alt_distance(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
return abs(loc1->alt - loc2->alt);
|
||||
}
|
||||
|
||||
long get_bearing(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
long off_x = loc2->lng - loc1->lng;
|
||||
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
|
||||
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
|
||||
if (bearing < 0) bearing += 36000;
|
||||
return bearing;
|
||||
}
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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"));
|
||||
}
|
|
@ -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;
|
|
@ -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
|
|
@ -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();
|
||||
}
|
||||
}
|
|
@ -0,0 +1,3 @@
|
|||
APM_ADC KEYWORD1
|
||||
Init KEYWORD2
|
||||
Ch KEYWORD2
|
|
@ -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;
|
|
@ -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
|
|
@ -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();
|
||||
}
|
||||
}
|
|
@ -0,0 +1,5 @@
|
|||
APM_BMP085 KEYWORD1
|
||||
Init KEYWORD2
|
||||
Read KEYWORD2
|
||||
Press KEYWORD2
|
||||
Temp KEYWORD2
|
|
@ -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;
|
||||
}
|
||||
}
|
|
@ -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
|
|
@ -0,0 +1,6 @@
|
|||
APM_BinComm KEYWORD1
|
||||
update KEYWORD2
|
||||
messagesReceived KEYWORD2
|
||||
badMessagesReceived KEYWORD2
|
||||
messagesSent KEYWORD2
|
||||
|
|
@ -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
|
||||
|
File diff suppressed because it is too large
Load Diff
|
@ -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++
|
||||
}
|
|
@ -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)
|
|
@ -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
|
|
@ -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();
|
||||
}
|
||||
}
|
|
@ -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;
|
|
@ -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
|
|
@ -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();
|
||||
}
|
||||
}
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
|
@ -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
|
||||
|
|
@ -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++;
|
||||
}
|
||||
}
|
|
@ -0,0 +1,2 @@
|
|||
APM_FastSerial KEYWORD1
|
||||
APM_FastSerial3 KEYWORD1
|
|
@ -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)
|
|
@ -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
|
|
@ -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();
|
||||
}
|
||||
}
|
|
@ -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
|
|
@ -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;
|
|
@ -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
|
|
@ -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();
|
||||
}
|
||||
}
|
|
@ -0,0 +1,5 @@
|
|||
APM_RC_QUAD KEYWORD1
|
||||
begin KEYWORD2
|
||||
InputCh KEYWORD2
|
||||
OutputCh KEYWORD2
|
||||
GetState KEYWORD2
|
|
@ -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"
|
||||
|
|
@ -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
|
|
@ -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);
|
||||
}
|
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
|
@ -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)
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
Menu KEYWORD1
|
||||
run KEYWORD2
|
||||
Location KEYWORD2
|
||||
|
|
@ -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]));
|
||||
}
|
|
@ -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);
|
||||
}
|
|
@ -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
|
|
@ -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
|
|
@ -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(" )");
|
||||
}
|
||||
}
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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"
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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());
|
||||
}
|
|
@ -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
|
|
@ -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;
|
||||
}
|
|
@ -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
|
|
@ -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;
|
||||
}
|
|
@ -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
|
|
@ -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;
|
||||
}
|
|
@ -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
|
|
@ -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
|
|
@ -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;
|
||||
}
|
|
@ -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
|
|
@ -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;
|
||||
}
|
|
@ -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
|
|
@ -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);
|
||||
}
|
||||
}
|
|
@ -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
Loading…
Reference in New Issue