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

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1191 f9c3cf11-9bcb-44bc-f272-b75c42450872

This commit is contained in:
analoguedevices 2010-12-19 16:40:33 +00:00
parent dfe94bc77e
commit 95d683d043
31 changed files with 8373 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopterMega/APM_Config.h Normal file
View File

@ -0,0 +1,13 @@
// Example config file. Use APM_Config.h.reference and the wiki to find additional defines to setup your plane.
// Once you upload the code, run the factory "reset" to save all config values to EEPROM.
// After reset, use the setup mode to set your radio limits for CH1-4, and to set your flight modes.
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK
#define ARM_AT_STARTUP 0
//#define MAGOFFSET 30.50,15.00,47.00
//#define DECLINATION 14.2

749
ArduCopterMega/APM_Config.h.reference Normal file
View File

@ -0,0 +1,749 @@
//
// 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
//
//////////////////////////////////////////////////////////////////////////////
// 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
//
// 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
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// 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
//
//////////////////////////////////////////////////////////////////////////////
// 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
//
//////////////////////////////////////////////////////////////////////////////
// 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
//
// PITCH_MAX OPTIONAL
//
// The maximum commanded pitch up angle.
// The default is 45 degrees.
//
//#define PITCH_MAX 45
//////////////////////////////////////////////////////////////////////////////
// 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
//
//
// 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.

838
ArduCopterMega/ArduCopterMega.pde Normal file
View File

@ -0,0 +1,838 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
ArduCopterMega Version 0.1 Experimental
Authors: Jason Short
Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
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.
*/
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <RC_Channel.h> // ArduPilot Mega RC Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_Compass_HMC5843.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // ArduPilot Mega RC Library
// 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)
// standard sensors for live flight
AP_ADC_ADS7844 adc;
APM_BMP085_Class APM_BMP085;
AP_Compass_HMC5843 compass;
// 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
AP_IMU imu(&adc, EE_IMU_OFFSET);
AP_DCM dcm(&imu, &GPS);
// GENERAL VARIABLE DECLARATIONS
// --------------------------------------------
byte control_mode = STABILIZE;
boolean failsafe = false; // did our throttle dip below the failsafe value?
boolean ch3_failsafe = false;
byte oldSwitchPosition; // for remembering the control mode switch
const char *comma = ",";
byte flight_modes[6];
const char* flight_mode_strings[] = {
"ACRO",
"STABILIZE",
"ALT_HOLD",
"AUTO",
"POSITION_HOLD",
"RTL",
"TAKEOFF",
"LAND"};
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
3 Throttle
4 Rudder (if we have ailerons)
5 Mode - 3 position switch
6 Altitude for Hold, user assignable
7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)
8 TBD
*/
// Radio
// -----
RC_Channel rc_1(EE_RADIO_1);
RC_Channel rc_2(EE_RADIO_2);
RC_Channel rc_3(EE_RADIO_3);
RC_Channel rc_4(EE_RADIO_4);
RC_Channel rc_5(EE_RADIO_5);
RC_Channel rc_6(EE_RADIO_6);
RC_Channel rc_7(EE_RADIO_7);
RC_Channel rc_8(EE_RADIO_8);
int motor_out[4];
byte flight_mode_channel;
byte frame_type = PLUS_FRAME;
// PIDs and gains
// ---------------
//Acro
PID pid_acro_rate_roll (EE_GAIN_1);
PID pid_acro_rate_pitch (EE_GAIN_2);
PID pid_acro_rate_yaw (EE_GAIN_3);
float acro_rate_roll_pitch, acro_rate_yaw;
//Stabilize
PID pid_stabilize_roll (EE_GAIN_4);
PID pid_stabilize_pitch (EE_GAIN_5);
PID pid_yaw (EE_GAIN_6);
float stabilize_rate_roll_pitch;
float stabilize_rate_yaw;
float stabilze_dampener;
int max_stabilize_dampener;
float stabilze_yaw_dampener;
int max_yaw_dampener;
// Nav
PID pid_nav (EE_GAIN_7);
PID pid_throttle (EE_GAIN_8);
// GPS variables
// -------------
byte ground_start_count = 5; // have we achieved first lock and set Home?
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 = 3; // meters
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 alt_to_hold; // how high we should be for RTL
long nav_angle;
long pitch_max;
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
float altitude_gain; // in nav
float distance_gain; // in nav
// Airspeed
// --------
int airspeed; // m/s * 100
// Throttle Failsafe
// ------------------
boolean motor_armed;
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 distance_error; // distance to the WP
long yaw_error; // how off are we pointed
// Sensors
// -------
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
// Magnetometer variables
// ----------------------
int magnetom_x;
int magnetom_y;
int magnetom_z;
float MAG_Heading;
float mag_offset_x;
float mag_offset_y;
float mag_offset_z;
float mag_declination;
bool compass_enabled;
// Barometer Sensor variables
// --------------------------
int baro_offset; // used to correct drift of absolute pressue sensor
unsigned long abs_pressure;
unsigned long abs_pressure_ground;
int ground_temperature;
int temp_unfilt;
// 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
// 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 nav_yaw; // deg * 100 : target yaw angle
int nav_throttle; // 0-1000 for throttle control
long command_yaw_start; // what angle were we to begin with
long command_yaw_start_time; // when did we start turning
int command_yaw_time; // how long we are turning
long command_yaw_end; // what angle are we trying to be
long command_yaw_delta; // how many degrees will we turn
int command_yaw_speed; // how fast to turn
byte command_yaw_dir;
long old_alt; // used for managing altitude rates
int velocity_land;
long altitude_estimate; // for smoothing GPS output
long distance_estimate; // for smoothing GPS output
int throttle_min; // 0 - 1000 : Min throttle output - copter should be 0
int throttle_cruise; // 0 - 1000 : what will make the copter hover
int throttle_max; // 0 - 1000 : Max throttle output
// Waypoints
// ---------
long GPS_wp_distance; // meters - distance between plane and next waypoint
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; // used to delay commands
long delay_start; // 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
boolean home_is_set = false; // Flag for if we have gps lock and have set the home location
// IMU variables
// -------------
float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
float COGX; // Course overground X axis
float COGY = 1; // Course overground Y axis
// 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];
char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
// System Timers
// --------------
unsigned long fast_loopTimer; // Time in miliseconds of main control loop
unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
int mainLoop_count;
unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
byte medium_loopCounter; // Counters for branching from main control loop to slower loops
byte medium_count;
byte slow_loopCounter;
byte superslow_loopCounter;
unsigned long deltaMiliSeconds; // Delta Time in miliseconds
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
unsigned long elapsedTime; // for doing custom events
float load; // % MCU cycles used
byte FastLoopGate = 9;
// Basic Initialization
//---------------------
void setup() {
init_ardupilot();
}
void loop()
{
// We want this to execute at 100Hz
// --------------------------------
if (millis() - fast_loopTimer > 9) {
deltaMiliSeconds = millis() - fast_loopTimer;
fast_loopTimer = millis();
load = float(fast_loopTimeStamp - fast_loopTimer) / deltaMiliSeconds;
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
fast_loopTimeStamp = millis();
}
if (millis() - medium_loopTimer > 19) {
medium_loopTimer = millis();
medium_loop();
/* commented out temporarily
if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
GCS.send_message(MSG_PERF_REPORT);
if (log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
resetPerfData();
}
}*/
}
}
// Main loop 50-100Hz
void fast_loop()
{
// IMU DCM Algorithm
read_AHRS();
// This is the fast loop - we want it to execute at 200Hz if possible
// ------------------------------------------------------------------
if (deltaMiliSeconds > G_Dt_max)
G_Dt_max = deltaMiliSeconds;
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
// write out the servo PWM values
// ------------------------------
set_servos_4();
}
void medium_loop()
{
// Read radio
// ----------
read_radio(); // read the radio first
// 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();
readCommands();
if(compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
}
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;
// command processing
//-------------------
case 2:
medium_loopCounter++;
// Read Baro pressure
// ------------------
read_barometer();
// altitude smoothing
// ------------------
calc_altitude_error();
// perform next command
// --------------------
update_commands();
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 == 0))
Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (int)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:
// shall we trim the copter?
// ------------------------
read_trim_switch();
// shall we check for engine start?
// --------------------------------
arm_motors();
medium_loopCounter = 0;
slow_loop();
break;
default:
medium_loopCounter = 0;
break;
}
// stuff that happens at 50 hz
// ---------------------------
// use Yaw to find our bearing error
calc_bearing_error();
// guess how close we are - fixed observer calc
calc_distance_error();
if (log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (int)yaw_sensor);
if (log_bitmask & MASK_LOG_RAW)
Log_Write_Raw();
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W
readgcsinput();
#endif
#if ENABLE_HIL
output_HIL();
#endif
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 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++;
//Serial.println(stabilize_rate_roll_pitch,3);
// Read 3-position switch on radio
// -------------------------------
read_control_switch();
//Serial.print("I: ")
//Serial.println(rc_1.get_integrator(), 1);
// Read main battery voltage if hooked up - does not read the 5v from radio
// ------------------------------------------------------------------------
#if BATTERY_EVENT == 1
read_battery();
#endif
break;
case 2:
slow_loopCounter = 0;
update_events();
break;
default:
slow_loopCounter = 0;
break;
}
}
void update_GPS(void)
{
GPS.update();
update_GPS_light();
if (GPS.new_data && GPS.fix) {
send_message(MSG_LOCATION);
// for performance
// ---------------
gps_fix_count++;
if(ground_start_count > 1){
ground_start_count--;
} 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) {
Serial.println("!! bad loc");
ground_start_count = 5;
} else {
if (log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
init_home();
// init altitude
current_loc.alt = GPS.altitude;
ground_start_count = 0;
}
}
/* disabled for now
// baro_offset is an integrator for the gps altitude error
baro_offset += altitude_gain * (float)(GPS.altitude - current_loc.alt);
*/
current_loc.lng = GPS.longitude; // Lon * 10 * *7
current_loc.lat = GPS.latitude; // Lat * 10 * *7
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){
//Serial.print("!");
//crash_checker();
switch(command_must_ID){
//case CMD_TAKEOFF:
// break;
//case CMD_LAND:
// break;
default:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
calc_nav_pid();
calc_nav_roll();
calc_nav_pitch();
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
// perform stabilzation
output_stabilize();
// hold yaw
//output_yaw_hold();
// apply throttle control
output_auto_throttle();
break;
}
}else{
switch(control_mode){
case STABILIZE:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
// get desired yaw control from radio
input_yaw_hold();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
// apply throttle control
output_manual_throttle();
// hold yaw
//output_yaw_hold();
// perform stabilzation
output_stabilize();
break;
case ALT_HOLD:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
// get desired height from the throttle
next_WP.alt = home.alt + (rc_3.control_in * 4) -100; // 0 - 1000 (40 meters)
// get desired yaw control from radio
input_yaw_hold();
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
// apply throttle control
output_auto_throttle();
// hold yaw
//output_yaw_hold();
// perform stabilzation
output_stabilize();
break;
case RTL:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
calc_nav_pid();
calc_nav_roll();
calc_nav_pitch();
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
// apply throttle control
output_auto_throttle();
// hold yaw
//output_yaw_hold();
// perform stabilzation
output_stabilize();
break;
case POSITION_HOLD:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
calc_nav_pid();
calc_nav_roll();
calc_nav_pitch();
// get desired yaw control from radio
input_yaw_hold();
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
// apply throttle control
output_auto_throttle();
// hold yaw
//output_yaw_hold();
// perform stabilzation
output_stabilize();
break;
default:
//Serial.print("$");
break;
}
}
}
// called after a GPS read
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 RTL:
update_crosstrack();
break;
}
}
}
void read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------------
dcm.update_DCM(G_Dt);
roll_sensor = dcm.roll_sensor;
pitch_sensor = dcm.pitch_sensor;
yaw_sensor = dcm.yaw_sensor;
}

230
ArduCopterMega/Attitude.pde Normal file
View File

@ -0,0 +1,230 @@
// desired angle in
// motor commands out (in degrees)
void init_pids()
{
max_stabilize_dampener = pid_stabilize_roll.kP() * 2500;
stabilze_dampener = 5729.57795 * stabilize_rate_roll_pitch;
max_yaw_dampener = pid_yaw.kP() * 6000; // .3 * 6000 = 1800
stabilze_yaw_dampener = 5729.57795 * stabilize_rate_yaw; // .3
}
void output_stabilize()
{
float roll_error, pitch_error;
int max_out;
Vector3f omega = dcm.get_gyro();
/*testing code:*/
//pitch_sensor = roll_sensor = 0; // testing only
//stabilize_rate_roll_pitch = (float)rc_6.control_in / 1000;
//init_pids();
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
rc_1.servo_out = rc_1.control_mix(nav_roll);
rc_2.servo_out = rc_2.control_mix(nav_pitch);
roll_error = rc_1.servo_out - roll_sensor;
pitch_error = rc_2.servo_out - pitch_sensor;
yaw_error = nav_yaw - yaw_sensor;
yaw_error = wrap_180(yaw_error);
// limit the error we're feeding to the PID
roll_error = constrain(roll_error, -2500, 2500);
pitch_error = constrain(pitch_error, -2500, 2500);
yaw_error = constrain(yaw_error, -6000, 6000);
//Serial.printf("s: %d \t mix %d, err %d", (int)roll_sensor, (int)rc_1.servo_out, (int)roll_error);
// write out angles back to servo out - this will be converted to PWM by RC_Channel
rc_1.servo_out = pid_stabilize_roll.get_pid(roll_error, deltaMiliSeconds, 1.0);
rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, deltaMiliSeconds, 1.0);
rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .3 = 198pwm
//Serial.printf("\tpid: %d", (int)rc_1.servo_out);
// We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates
// omega is the raw gyro reading
int roll_dampener = (omega.x * stabilze_dampener);// Omega is in radians
int pitch_dampener = (omega.y * stabilze_dampener);
int yaw_dampener = (omega.z * stabilze_yaw_dampener);
// Limit dampening to be equal to propotional term for symmetry
rc_1.servo_out -= constrain(roll_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15°
rc_2.servo_out -= constrain(pitch_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15°
rc_4.servo_out -= constrain(yaw_dampener, -max_yaw_dampener, max_yaw_dampener); // +- 15°
//Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener);
//Serial.printf("\trd: %d", roll_dampener);
//Serial.printf("\tlimit: %d, PWM: %d", rc_1.servo_out, rc_1.angle_to_pwm());
}
// err -2500 pid: -1100 rd: 1117 limit: -1650, PWM: -152
//s: -1247 mix 0, err 1247 pid: 548 rd: -153 limit: 395, PWM: 35
void output_rate_control()
{
Vector3f omega = dcm.get_gyro();
rc_4.servo_out = rc_4.control_in;
rc_1.servo_out = rc_2.control_in;
rc_2.servo_out = rc_2.control_in;
// Rate control through bias corrected gyro rates
// omega is the raw gyro reading plus Omega_I, so it´s bias corrected
rc_1.servo_out -= (omega.x * 5729.57795 * acro_rate_roll_pitch);
rc_2.servo_out -= (omega.y * 5729.57795 * acro_rate_roll_pitch);
rc_4.servo_out -= (omega.z * 5729.57795 * acro_rate_yaw);
//Serial.printf("\trated out %d, omega ", rc_1.servo_out);
//Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3);
// Limit output
rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_2.servo_out = constrain(rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_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)
{
pid_nav.reset_I();
pid_throttle.reset_I();
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void set_servos_4(void)
{
static byte num;
//motor_armed = false;
// Quadcopter mix
if (motor_armed == true) {
int out_min = rc_3.radio_min;
// Throttle is 0 to 1000 only
rc_3.servo_out = constrain(rc_3.servo_out, 0, 1000);
if(rc_3.servo_out > 0)
out_min = rc_3.radio_min + 50;
//Serial.printf("out: %d %d %d %d\t\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out);
// creates the radio_out anf pwm_out values
rc_1.calc_pwm();
rc_2.calc_pwm();
rc_3.calc_pwm();
rc_4.calc_pwm();
//Serial.printf("out: %d %d %d %d\n", rc_1.radio_out, rc_2.radio_out, rc_3.radio_out, rc_4.radio_out);
//Serial.printf("yaw: %d ", rc_4.radio_out);
if(frame_type == PLUS_FRAME){
motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out;
motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out;
motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out;
motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out;
}else{
int roll_out = rc_1.pwm_out / 2;
int pitch_out = rc_2.pwm_out / 2;
motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out;
motor_out[LEFT] = rc_3.radio_out + roll_out - pitch_out;
motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out;
motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out;
}
//Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
motor_out[RIGHT] += rc_4.pwm_out;
motor_out[LEFT] += rc_4.pwm_out;
motor_out[FRONT] -= rc_4.pwm_out;
motor_out[BACK] -= rc_4.pwm_out;
//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
motor_out[RIGHT] = constrain(motor_out[RIGHT], out_min, rc_3.radio_max);
motor_out[LEFT] = constrain(motor_out[LEFT], out_min, rc_3.radio_max);
motor_out[FRONT] = constrain(motor_out[FRONT], out_min, rc_3.radio_max);
motor_out[BACK] = constrain(motor_out[BACK], out_min, rc_3.radio_max);
///*
int r_out = ((long)(motor_out[RIGHT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min);
int l_out = ((long)(motor_out[LEFT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min);
int f_out = ((long)(motor_out[FRONT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min);
int b_out = ((long)(motor_out[BACK] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min);
//*/
//~#*set_servos_4: 398, -39 38 38 -36
/*
num++;
if (num > 50){
num = 0;
Serial.printf("control_in: %d ", rc_3.control_in);
Serial.printf(" servo: %d %d %d %d\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out);
Serial.printf(" pwm: %d %d %d %d\n", r_out, l_out, f_out, b_out);
}
//*/
//Serial.printf("set: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
//Serial.printf("s: %d %d %d\t\t", (int)roll_sensor, (int)pitch_sensor, (int)yaw_sensor);
///Serial.printf("outmin: %d\n", out_min);
/*
write_int(r_out);
write_int(l_out);
write_int(f_out);
write_int(b_out);
write_int((int)(roll_sensor / 100));
write_int((int)(pitch_sensor / 100));
write_int((int)(yaw_sensor / 100));
write_int((int)(yaw_error / 100));
write_int((int)(current_loc.alt));
write_int((int)(altitude_error));
flush(10);
//*/
// Send commands to motors
if(rc_3.servo_out > 0){
APM_RC.OutputCh(CH_1, motor_out[RIGHT]);
APM_RC.OutputCh(CH_2, motor_out[LEFT]);
APM_RC.OutputCh(CH_3, motor_out[FRONT]);
APM_RC.OutputCh(CH_4, motor_out[BACK]);
}else{
APM_RC.OutputCh(CH_1, rc_3.radio_min);
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
}
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}else{
// Send commands to motors
APM_RC.OutputCh(CH_1, rc_3.radio_min);
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
// reset I terms of PID controls
reset_I();
// Initialize yaw command to actual yaw when throttle is down...
rc_4.control_in = ToDeg(yaw);
}
}
void demo_servos(byte i) {
// nothing to do
}

389
ArduCopterMega/EEPROM map.txt Normal file
View File

@ -0,0 +1,389 @@
0 0000 16 int radio_trim 1
1 0001 ..
2 0002 16 int radio_trim 2
3 0003 ..
4 0004 16 int radio_trim 3
5 0005 ..
6 0006 16 int radio_trim 4
7 0007 ..
8 0008 16 int radio_trim 5
9 0009 ..
10 000A 16 int radio_trim 6
11 000B ..
12 000C 16 int radio_trim 7
13 000D ..
14 000E 16 int radio_trim 8
15 000F ..
16 0010 16 int radio_min 1
17 0011 ..
18 0012 16 int radio_min 2
19 0013 ..
20 0014 16 int radio_min 3
21 0015 ..
22 0016 16 int radio_min 4
23 0017 ..
24 0018 16 int radio_min 5
25 0019 ..
26 001A 16 int radio_min 6
27 001B ..
28 001C 16 int radio_min 7
29 001D ..
30 001E 16 int radio_min 8
31 001F ..
32 0020 16 int radio_max 1
33 0021 ..
34 0022 16 int radio_max 2
35 0023 ..
36 0024 16 int radio_max 3
37 0025 ..
38 0026 16 int radio_max 4
39 0027 ..
40 0028 16 int radio_max 5
41 0029 ..
42 002A 16 int radio_max 6
43 002B ..
44 002C 16 int radio_max 7
45 002D ..
46 002E 16 int radio_max 8
47 002F ..
48 0030 16 int elevon_trim 1
49 0031 ..
50 0032 16 int elevon_trim 2
51 0033 ..
52 0034 16 int XTRACK_GAIN
53 0035 ..
54 0036 16 int XTRACK_ENTRY_ANGLE
55 0037 ..
56 0038 8 int HEAD_MAX
57 0039 8 int PITCH_MAX
58 003A 8 int Pitch_min
59 003B 32 float EE_ALT_MIX
60 003C ..
61 003D ..
62 003E ..
63 003F
64 0040 32 float Kp 0
65 0041 ..
66 0042 ..
67 0043 ..
68 0044 32 float Kp 1
69 0045 ..
70 0046 ..
71 0047 ..
72 0048 32 float Kp 2
73 0049 ..
74 004A ..
75 004B ..
76 004C 32 float Kp 3
77 004D ..
78 004E ..
79 004F ..
80 0050 32 float Kp 4
81 0051 ..
82 0052 ..
83 0053 ..
84 0054 32 float Kp 5
85 0055 ..
86 0056 ..
87 0057 ..
88 0058 32 float Kp 6
89 0059 ..
90 005A ..
91 005B ..
92 005C 32 float Kp 7
93 005D ..
94 005E ..
95 005F ..
96 0060 32 float Ki 0
97 0061 ..
98 0062 ..
99 0063 ..
100 0064 32 float Ki 1
101 0065 ..
102 0066 ..
103 0067 ..
104 0068 32 float Ki 2
105 0069 ..
106 006A ..
107 006B ..
108 006C 32 float Ki 3
109 006D ..
110 006E ..
111 006F ..
112 0070 32 float Ki 4
113 0071 ..
114 0072 ..
115 0073 ..
116 0074 32 float Ki 5
117 0075 ..
118 0076 ..
119 0077 ..
120 0078 32 float Ki 6
121 0079 ..
122 007A ..
123 007B ..
124 007C 32 float Ki 7
125 007D ..
126 007E ..
127 007F ..
128 0080 32 float kd 0
129 0081 ..
130 0082 ..
131 0083 ..
132 0084 32 float kd 1
133 0085 ..
134 0086 ..
135 0087 ..
136 0088 32 float kd 2
137 0089 ..
138 008A ..
139 008B ..
140 008C 32 float kd 3
141 008D ..
142 008E ..
143 008F ..
144 0090 32 float kd 4
145 0091 ..
146 0092 ..
147 0093 ..
148 0094 32 float kd 5
149 0095 ..
150 0096 ..
151 0097 ..
152 0098 32 float kd 6
153 0099 ..
154 009A ..
155 009B ..
156 009C 32 float kd 7
157 009D ..
158 009E ..
159 009F ..
160 00A0 32 float integrator_max 0
161 00A1 ..
162 00A2 ..
163 00A3 ..
164 00A4 32 float integrator_max 1
165 00A5 ..
166 00A6 ..
167 00A7 ..
168 00A8 32 float integrator_max 2
169 00A9 ..
170 00AA ..
171 00AB ..
172 00AC 32 float integrator_max 3
173 00AD ..
174 00AE ..
175 00AF ..
176 00B0 32 float integrator_max 4
177 00B1 ..
178 00B2 ..
179 00B3 ..
180 00B4 32 float integrator_max 5
181 00B5 ..
182 00B6 ..
183 00B7 ..
184 00B8 32 float integrator_max 6
185 00B9 ..
186 00BA ..
187 00BB ..
188 00BC 32 float integrator_max 7
189 00BD ..
190 00BE ..
191 00BF ..
192 00C0 32 float Kff 0
193 00C1 ..
194 00C2 ..
195 00C3 ..
196 00C4 32 float Kff 1
197 00C5 ..
198 00C6 ..
199 00C7 ..
200 00C8 32 float Kff 2
201 00C9 ..
202 00CA ..
203 00CB ..
204 00CC 32 float Kff 3
205 00CD ..
206 00CE ..
207 00CF ..
208 00D0 32 float Kff 4
209 00D1 ..
210 00D2 ..
211 00D3 ..
212 00D4 32 float Kff 5
213 00D5 ..
214 00D6 ..
215 00D7 ..
216 00D8 32 float Kff 6
217 00D9 ..
218 00DA ..
219 00DB ..
220 00DC 32 float Kff 7
221 00DD ..
222 00DE ..
223 00DF ..
224 00E0 32 int EE_ACCEL_OFFSET 0
225 00E1 ..
226 00E2 ..
227 00E3 ..
228 00E4 32 int EE_ACCEL_OFFSET 1
229 00E5 ..
230 00E6 ..
231 00E7 ..
232 00E8 32 int EE_ACCEL_OFFSET 2
233 00E9 ..
234 00EA ..
235 00EB ..
236 00EC 32 int EE_ACCEL_OFFSET 3
237 00ED ..
238 00EE ..
239 00EF ..
240 00F0 32 int EE_ACCEL_OFFSET 4
241 00F1 ..
242 00F2 ..
243 00F3 ..
244 00F4 32 int EE_ACCEL_OFFSET 5
245 00F5 ..
246 00F6 ..
247 00F7 ..
248 00F8 8 EE_CONFIG
249 00F9 8 EE_WP_MODE
250 00FA 8 EE_YAW_MODE
251 00FB 8 EE_WP_TOTAL
252 00FC 8 EE_WP_INDEX
253 00FD 8 EE_WP_RADIUS
254 00FE 8 EE_LOITER_RADIUS
255 00FF 32 EE_ALT_HOLD_HOME
256 0100 ..
257 0101 ..
258 0102 ..
259 0103 8 AIRSPEED_CRUISE
260 0104
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
275 0113
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 ..

360
ArduCopterMega/EEPROM.pde Normal file
View File

@ -0,0 +1,360 @@
// -*- 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
// ************************************************************************************
// Macro functions
// ---------------
void read_EEPROM_startup(void)
{
read_EEPROM_PID();
read_EEPROM_radio(); // read Radio limits
read_EEPROM_frame();
read_EEPROM_configs();
read_EEPROM_flight_modes();
read_EEPROM_waypoint_info();
imu.load_gyro_eeprom();
imu.load_accel_eeprom();
read_EEPROM_alt_RTL();
// magnatometer
read_EEPROM_mag();
read_EEPROM_mag_declination();
read_EEPROM_mag_offset();
}
void read_EEPROM_airstart_critical(void)
{
int16_t temp = 0;
imu.load_gyro_eeprom();
imu.load_accel_eeprom();
// Read the home location
//-----------------------
home = get_wp_with_index(0);
// Read pressure sensor values
//----------------------------
read_EEPROM_pressure();
}
void save_EEPROM_groundstart(void)
{
rc_1.save_trim();
rc_2.save_trim();
rc_3.save_trim();
rc_4.save_trim();
rc_5.save_trim();
rc_6.save_trim();
rc_7.save_trim();
rc_8.save_trim();
// pressure sensor data saved by init_home
}
/********************************************************************************/
long read_alt_to_hold()
{
read_EEPROM_alt_RTL();
if(alt_to_hold == -1)
return current_loc.alt;
else
return alt_to_hold + home.alt;
}
/********************************************************************************/
void save_EEPROM_alt_RTL(void)
{
eeprom_write_dword((uint32_t *)EE_ALT_HOLD_HOME, alt_to_hold);
}
void read_EEPROM_alt_RTL(void)
{
alt_to_hold = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME);
}
/********************************************************************************/
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_nav(void)
{
// for nav estimation
distance_gain = read_EE_compressed_float(EE_DISTANCE_GAIN, 4);
altitude_gain = read_EE_compressed_float(EE_ALTITUDE_GAIN, 4);
// stored as degree * 100
x_track_gain = read_EE_compressed_float(EE_XTRACK_GAIN, 4);
x_track_angle = eeprom_read_word((uint16_t *) EE_XTRACK_ANGLE) * 100;
pitch_max = eeprom_read_word((uint16_t *) EE_PITCH_MAX); // scale to degress * 100
}
void save_EEPROM_nav(void)
{
// for nav estimation
write_EE_compressed_float(altitude_gain, EE_ALTITUDE_GAIN, 4);
write_EE_compressed_float(distance_gain, EE_DISTANCE_GAIN, 4);
write_EE_compressed_float(x_track_gain, EE_XTRACK_GAIN, 4);
// stored as degrees
eeprom_write_word((uint16_t *) EE_XTRACK_ANGLE, x_track_angle / 100);
// stored as degrees
eeprom_write_word((uint16_t *) EE_PITCH_MAX, pitch_max);
}
/********************************************************************************/
void read_EEPROM_PID(void)
{
pid_acro_rate_roll.load_gains();
pid_acro_rate_pitch.load_gains();
pid_acro_rate_yaw.load_gains();
pid_stabilize_roll.load_gains();
pid_stabilize_pitch.load_gains();
pid_yaw.load_gains();
pid_nav.load_gains();
pid_throttle.load_gains();
stabilize_rate_roll_pitch = read_EE_compressed_float(EE_STAB_RATE_RP, 4);
stabilize_rate_yaw = read_EE_compressed_float(EE_STAB_RATE_YAW, 4);
acro_rate_roll_pitch = read_EE_compressed_float(EE_ACRO_RATE_RP, 4);
acro_rate_yaw = read_EE_compressed_float(EE_ACRO_RATE_YAW, 4);
}
void save_EEPROM_PID(void)
{
pid_acro_rate_roll.save_gains();
pid_acro_rate_pitch.save_gains();
pid_acro_rate_yaw.save_gains();
pid_stabilize_roll.save_gains();
pid_stabilize_pitch.save_gains();
pid_yaw.save_gains();
pid_nav.save_gains();
pid_throttle.save_gains();
write_EE_compressed_float(stabilize_rate_roll_pitch, EE_STAB_RATE_RP, 4);
write_EE_compressed_float(stabilize_rate_yaw, EE_STAB_RATE_YAW, 4);
write_EE_compressed_float(acro_rate_roll_pitch, EE_ACRO_RATE_RP, 4);
write_EE_compressed_float(acro_rate_yaw, EE_ACRO_RATE_YAW, 4);
}
/********************************************************************************/
void save_EEPROM_frame(void)
{
eeprom_write_byte((uint8_t *)EE_FRAME, frame_type);
}
void read_EEPROM_frame(void)
{
frame_type = eeprom_read_byte((uint8_t *) EE_FRAME);
}
/********************************************************************************/
void save_EEPROM_throttle_cruise(void)
{
eeprom_write_word((uint16_t *)EE_THROTTLE_CRUISE, throttle_cruise);
}
void read_EEPROM_throttle_cruise(void)
{
throttle_cruise = eeprom_read_word((uint16_t *) EE_FRAME);
}
/********************************************************************************/
void save_EEPROM_mag_declination(void)
{
write_EE_compressed_float(mag_declination, EE_MAG_DECLINATION, 1);
}
void read_EEPROM_mag_declination(void)
{
mag_declination = read_EE_compressed_float(EE_MAG_DECLINATION, 1);
}
/********************************************************************************/
void save_EEPROM_mag_offset(void)
{
write_EE_compressed_float(mag_offset_x, EE_MAG_X, 2);
write_EE_compressed_float(mag_offset_y, EE_MAG_Y, 2);
write_EE_compressed_float(mag_offset_z, EE_MAG_Z, 2);
}
void read_EEPROM_mag_offset(void)
{
mag_offset_x = read_EE_compressed_float(EE_MAG_X, 2);
mag_offset_y = read_EE_compressed_float(EE_MAG_Y, 2);
mag_offset_z = read_EE_compressed_float(EE_MAG_Z, 2);
}
/********************************************************************************/
void read_EEPROM_mag(void)
{
compass_enabled = eeprom_read_byte((uint8_t *) EE_COMPASS);
}
void save_EEPROM_mag(void)
{
eeprom_write_byte((uint8_t *) EE_COMPASS, compass_enabled);
}
/********************************************************************************/
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_EEPROM_pressure(void)
{
eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, abs_pressure_ground);
eeprom_write_word((uint16_t *)EE_GND_TEMP, ground_temperature);
}
void read_EEPROM_pressure(void)
{
abs_pressure_ground = eeprom_read_dword((uint32_t *) EE_ABS_PRESS_GND);
// Better than zero for an air start value
abs_pressure = abs_pressure_ground;
ground_temperature = eeprom_read_word((uint16_t *) EE_GND_TEMP);
}
/********************************************************************************/
void read_EEPROM_radio(void)
{
rc_1.load_eeprom();
rc_2.load_eeprom();
rc_3.load_eeprom();
rc_4.load_eeprom();
rc_5.load_eeprom();
rc_6.load_eeprom();
rc_7.load_eeprom();
rc_8.load_eeprom();
}
void save_EEPROM_radio(void)
{
rc_1.save_eeprom();
rc_2.save_eeprom();
rc_3.save_eeprom();
rc_4.save_eeprom();
rc_5.save_eeprom();
rc_6.save_eeprom();
rc_7.save_eeprom();
rc_8.save_eeprom();
}
/********************************************************************************/
void read_EEPROM_configs(void)
{
throttle_min = eeprom_read_word((uint16_t *) EE_THROTTLE_MIN);
throttle_max = eeprom_read_word((uint16_t *) EE_THROTTLE_MAX);
read_EEPROM_throttle_cruise();
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);
log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
}
void save_EEPROM_configs(void)
{
eeprom_write_word((uint16_t *) EE_THROTTLE_MIN, throttle_min);
eeprom_write_word((uint16_t *) EE_THROTTLE_MAX, throttle_max);
save_EEPROM_throttle_cruise();
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_word((uint16_t *) EE_LOG_BITMASK, log_bitmask);
}
/********************************************************************************/
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));
}
/********************************************************************************/
float
read_EE_float(int address)
{
union {
byte bytes[4];
float value;
} _floatOut;
for (int i = 0; i < 4; i++)
_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
return _floatOut.value;
}
void write_EE_float(float value, int address)
{
union {
byte bytes[4];
float value;
} _floatIn;
_floatIn.value = value;
for (int i = 0; i < 4; i++)
eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
}
/********************************************************************************/
float
read_EE_compressed_float(int address, byte places)
{
float scale = pow(10, places);
int temp = eeprom_read_word((uint16_t *) address);
return ((float)temp / scale);
}
void write_EE_compressed_float(float value, int address, byte places)
{
float scale = pow(10, places);
int temp = value * scale;
eeprom_write_word((uint16_t *) address, temp);
}

100
ArduCopterMega/GCS.pde Normal file
View File

@ -0,0 +1,100 @@
/*
GCS Protocol
4 Ardupilot Header
D
5 Payload length
1 Message ID
1 Message Version
9 Payload byte 1
8 Payload byte 2
7 Payload byte 3
A Checksum byte 1
B Checksum byte 2
*/
#if GCS_PORT == 3
# define SendSerw Serial3.write
# define SendSer Serial3.print
#else
# define SendSerw Serial.write
# define SendSer Serial.print
#endif
byte mess_buffer[54];
byte buff_pointer;
// Unions for getting byte values
union f_out{
byte bytes[4];
float value;
} floatOut;
union i_out {
byte bytes[2];
int value;
} intOut;
union l_out{
byte bytes[4];
long value;
} longOut;
// Add binary values to the buffer
void write_byte(byte val)
{
mess_buffer[buff_pointer++] = val;
}
void write_int(int val )
{
intOut.value = val;
mess_buffer[buff_pointer++] = intOut.bytes[0];
mess_buffer[buff_pointer++] = intOut.bytes[1];
}
void write_float(float val)
{
floatOut.value = val;
mess_buffer[buff_pointer++] = floatOut.bytes[0];
mess_buffer[buff_pointer++] = floatOut.bytes[1];
mess_buffer[buff_pointer++] = floatOut.bytes[2];
mess_buffer[buff_pointer++] = floatOut.bytes[3];
}
void write_long(long val)
{
longOut.value = val;
mess_buffer[buff_pointer++] = longOut.bytes[0];
mess_buffer[buff_pointer++] = longOut.bytes[1];
mess_buffer[buff_pointer++] = longOut.bytes[2];
mess_buffer[buff_pointer++] = longOut.bytes[3];
}
void flush(byte id)
{
byte mess_ck_a = 0;
byte mess_ck_b = 0;
byte i;
SendSer("4D"); // This is the message preamble
SendSerw(buff_pointer); // Length
SendSerw(2); // id
SendSerw(0x01); // Version
for (i = 0; i < buff_pointer; i++) {
SendSerw(mess_buffer[i]);
}
//for (i = 0; i < buff_pointer; i++) {
// mess_ck_a += mess_buffer[i]; // Calculates checksums
// mess_ck_b += mess_ck_a;
//}
//SendSerw(mess_ck_a);
//SendSerw(mess_ck_b);
buff_pointer = 0;
}

146
ArduCopterMega/GCS_Ardupilot.pde Normal file
View File

@ -0,0 +1,146 @@
// -*- 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(rc_3.servo_out, DEC);
SendSer (",RLL:");
SendSer(roll_sensor / 100, DEC);
SendSer (",PCH:");
SendSer(pitch_sensor / 100, DEC);
SendSerln(",***");
}
void print_control_mode(void)
{
SendSer("###");
SendSer(flight_mode_strings[control_mode]);
SendSerln("***");
}
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(rc_1.servo_out/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

192
ArduCopterMega/GCS_IMU_ouput.pde Normal file
View File

@ -0,0 +1,192 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_IMU
// This is the standard GCS output file for ArduPilot
/*
Message Prefixes
!!! Position Low rate telemetry
+++ Attitude High rate telemetry
### Mode Change in control mode
%%% Waypoint Current and previous waypoints
XXX Alert Text alert - NOTE: Alert message generation is not localized to a function
PPP IMU Performance Sent every 20 seconds for performance monitoring
Message Suffix
*** All messages use this suffix
*/
/*
void acknowledge(byte id, byte check1, byte check2) {}
void send_message(byte id) {}
void send_message(byte id, long param) {}
void send_message(byte severity, const char *str) {}
*/
void acknowledge(byte id, byte check1, byte check2)
{
}
void send_message(byte id)
{
send_message(id,0l);
}
void send_message(byte id, long param)
{
switch(id) {
case MSG_ATTITUDE:
print_attitude();
break;
case MSG_LOCATION: // ** Location/GPS message
print_location();
break;
}
}
void send_message(byte severity, const char *str)
{
if(severity >= DEBUG_LEVEL){
Serial.print("MSG: ");
Serial.println(str);
}
}
void print_current_waypoints()
{
}
void print_control_mode(void)
{
}
void print_attitude(void)
{
//Serial.print("!!!VER:");
//Serial.print(SOFTWARE_VER); //output the software version
//Serial.print(",");
// Analogs
Serial.print("AN0:");
Serial.print(read_adc(0)); //Reversing the sign.
Serial.print(",AN1:");
Serial.print(read_adc(1));
Serial.print(",AN2:");
Serial.print(read_adc(2));
Serial.print(",AN3:");
Serial.print(read_adc(3));
Serial.print (",AN4:");
Serial.print(read_adc(4));
Serial.print (",AN5:");
Serial.print(read_adc(5));
Serial.print (",");
// DCM
Serial.print ("EX0:");
Serial.print(convert_to_dec(DCM_Matrix[0][0]));
Serial.print (",EX1:");
Serial.print(convert_to_dec(DCM_Matrix[0][1]));
Serial.print (",EX2:");
Serial.print(convert_to_dec(DCM_Matrix[0][2]));
Serial.print (",EX3:");
Serial.print(convert_to_dec(DCM_Matrix[1][0]));
Serial.print (",EX4:");
Serial.print(convert_to_dec(DCM_Matrix[1][1]));
Serial.print (",EX5:");
Serial.print(convert_to_dec(DCM_Matrix[1][2]));
Serial.print (",EX6:");
Serial.print(convert_to_dec(DCM_Matrix[2][0]));
Serial.print (",EX7:");
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
Serial.print (",EX8:");
Serial.print(convert_to_dec(DCM_Matrix[2][2]));
Serial.print (",");
// Euler
Serial.print("RLL:");
Serial.print(ToDeg(roll));
Serial.print(",PCH:");
Serial.print(ToDeg(pitch));
Serial.print(",YAW:");
Serial.print(ToDeg(yaw));
Serial.print(",IMUH:");
Serial.print(((int)imu_health>>8)&0xff);
Serial.print (",");
/*
#if USE_MAGNETOMETER == 1
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(pressure_altitude/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
ArduCopterMega/GCS_Jason_text.pde Normal file
View File

367
ArduCopterMega/GCS_Standard.pde Normal file
View File

@ -0,0 +1,367 @@
// -*- 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 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 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;
*/
}
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[0] = length; // Message length
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) {
Serial.println(str);
}
void print_current_waypoints(){}
void print_waypoint(struct Location *cmd, byte index){}
void print_waypoints(){}
#endif

127
ArduCopterMega/GCS_Xplane.pde Normal file
View File

@ -0,0 +1,127 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE
void acknowledge(byte id, byte check1, byte check2)
{
}
void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
{
if(severity >= DEBUG_LEVEL){
Serial.print("MSG: ");
Serial.println(str);
}
}
void send_message(byte id) {
send_message(id,0l);
}
void send_message(byte id, long param) {
switch(id) {
case MSG_HEARTBEAT:
print_control_mode();
break;
case MSG_ATTITUDE:
//print_attitude();
break;
case MSG_LOCATION:
//print_position();
break;
case MSG_COMMAND:
struct Location cmd = get_wp_with_index(param);
print_waypoint(&cmd, param);
break;
}
}
// required by Groundstation to plot lateral tracking course
void print_new_wp_info()
{
}
void print_control_mode(void)
{
Serial.print("MSG: ");
Serial.print(flight_mode_strings[control_mode]);
}
void print_current_waypoints()
{
Serial.print("MSG: ");
Serial.print("CUR:");
Serial.print("\t");
Serial.print(current_loc.lat,DEC);
Serial.print(",\t");
Serial.print(current_loc.lng,DEC);
Serial.print(",\t");
Serial.println(current_loc.alt,DEC);
Serial.print("MSG: ");
Serial.print("NWP:");
Serial.print(wp_index,DEC);
Serial.print(",\t");
Serial.print(next_WP.lat,DEC);
Serial.print(",\t");
Serial.print(next_WP.lng,DEC);
Serial.print(",\t");
Serial.println(next_WP.alt,DEC);
}
void print_position(void)
{
}
void printPerfData(void)
{
}
void print_attitude(void)
{
}
void print_tuning(void) {
}
void print_waypoint(struct Location *cmd,byte index)
{
Serial.print("MSG: command #: ");
Serial.print(index, DEC);
Serial.print(" id: ");
Serial.print(cmd->id,DEC);
Serial.print(" p1: ");
Serial.print(cmd->p1,DEC);
Serial.print(" p2: ");
Serial.print(cmd->alt,DEC);
Serial.print(" p3: ");
Serial.print(cmd->lat,DEC);
Serial.print(" p4: ");
Serial.println(cmd->lng,DEC);
}
void print_waypoints()
{
Serial.println("Commands in memory");
Serial.print("commands total: ");
Serial.println(wp_total, DEC);
// create a location struct to hold the temp Waypoints for printing
//Location tmp;
Serial.println("Home: ");
struct Location cmd = get_wp_with_index(0);
print_waypoint(&cmd, 0);
Serial.println("Commands: ");
for (int i=1; i<wp_total; i++){
cmd = get_wp_with_index(i);
print_waypoint(&cmd, i);
}
}
#endif

78
ArduCopterMega/HIL_output.pde Normal file
View File

@ -0,0 +1,78 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#if ENABLE_HIL
// An Xplane/Flightgear output
// Use with the GPS_IMU to do Hardware in teh loop simulations
byte buf_len = 0;
byte out_buffer[32];
void output_HIL(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(rc_4.servo_out)); // 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)(rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(rc_4.servo_out)); // 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

582
ArduCopterMega/Log.pde Normal file
View File

@ -0,0 +1,582 @@
// -*- 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_EEPROM_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(dcm.gyro_sat_count);
DataFlash.WriteByte(imu.adc_constraints);
DataFlash.WriteByte(dcm.renorm_sqrt_count);
DataFlash.WriteByte(dcm.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(dcm.get_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()
{
// DCM is adjusted for centripital, IMU is not
Vector3f accel = dcm.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(rc_1.servo_out));
DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)roll_sensor);
DataFlash.WriteInt((int)(rc_2.servo_out));
DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)pitch_sensor);
DataFlash.WriteInt((int)(rc_3.servo_out));
DataFlash.WriteInt((int)(rc_4.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
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()
{
Vector3f gyro = dcm.get_gyro();
Vector3f accel = dcm.get_accel();
gyro *= t7; // Scale up for storage as long integers
accel *= t7;
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
DataFlash.WriteLong((long)gyro.x);
DataFlash.WriteLong((long)gyro.y);
DataFlash.WriteLong((long)gyro.z);
DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z);
DataFlash.WriteByte(END_BYTE);
}
// 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 < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.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.println(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:");
Serial.println(flight_mode_strings[control_mode]);
}
// 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 = (float)DataFlash.ReadLong()/t7;
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);
}

79
ArduCopterMega/command description.txt Normal file
View File

@ -0,0 +1,79 @@
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 altitude - -
0x17 CMD_ALTITUDE - altitude - -
0x18 CMD_R_WAYPOINT - altitude rlat rlon
***********************************
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 pitch deg airspeed m/s, throttle %, distance to WP
0x23 CMD_ANGLE angle speed direction (-1,1) rel (1), abs (0)
***********************************
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

230
ArduCopterMega/commands.pde Normal file
View File

@ -0,0 +1,230 @@
// -*- 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++;
Serial.print("MSG WP index is incremented to ");
Serial.println(wp_index,DEC);
}else{
Serial.print("MSG Failed to increment WP index of ");
Serial.println(wp_index,DEC);
}
}
void decrement_WP_index()
{
if(wp_index > 0){
wp_index--;
}
}
//********************************************************************************
//This function sets the waypoint and modes for Return to Launch
//********************************************************************************
// 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;
}
void
set_current_loc_here()
{
//struct Location temp;
set_next_WP(&current_loc);
}
/*
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");
Serial.print("MSG set_next_WP, wp_index: ");
Serial.println(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 - current_loc.alt;
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
loiter_sum = 0;
loiter_total = 0;
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
//377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = getDistance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
print_current_waypoints();
target_bearing = get_bearing(&current_loc, &next_WP);
old_target_bearing = target_bearing;
// this is used to offset the shrinking longitude as we go towards the poles
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
// run this at setup on the ground
// -------------------------------
void init_home()
{
Serial.println("MSG: init home");
// Extra read just in case
// -----------------------
//GPS.Read();
// block until we get a good fix
// -----------------------------
while (!GPS.new_data || !GPS.fix) {
GPS.update();
}
home.id = CMD_WAYPOINT;
home.lng = GPS.longitude; // Lon * 10**7
home.lat = GPS.latitude; // Lat * 10**7
home.alt = GPS.altitude;
home_is_set = true;
// ground altitude in centimeters for pressure alt calculations
// ------------------------------------------------------------
//ground_alt = GPS.altitude;
//pressure_altitude = GPS.altitude; // Set initial value for filter
save_EEPROM_pressure();
// Save Home to EEPROM
// -------------------
set_wp_with_index(home, 0);
// Save prev loc
// -------------
prev_WP = home;
}

466
ArduCopterMega/commands_process.pde Normal file
View File

@ -0,0 +1,466 @@
// 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){
//Serial.print("MSG fetch found new cmd from list at index: ");
//Serial.println((wp_index+1),DEC);
//Serial.print("MSG cmd id: ");
//Serial.println(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!");
//Serial.print("MSG out of commands, wp_index: ");
//Serial.println(wp_index,DEC);
switch (control_mode){
case LAND:
// don't get a new command
break;
default:
next_command = get_LOITER_home_wp();
//Serial.print("MSG Preload RTL cmd id: ");
//Serial.println(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;
//Serial.print("MSG new command_must_id ");
//Serial.print(next_command.id,DEC);
//Serial.print(" index:");
//Serial.println(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()
{
//Serial.print("process must index: ");
//Serial.println(command_must_index,DEC);
send_message(SEVERITY_LOW,"New cmd: ");
send_message(MSG_COMMAND, wp_index);
// clear May indexes
command_may_index = 0;
command_may_ID = 0;
command_must_ID = next_command.id;
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
// reset navigation integrators
// -------------------------
reset_I();
// loads the waypoint into Next_WP struct
// --------------------------------------
set_next_WP(&next_command);
switch(command_must_ID){
case CMD_TAKEOFF: // TAKEOFF!
takeoff_altitude = (int)next_command.alt;
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
break;
case CMD_WAYPOINT: // Navigate to Waypoint
break;
case CMD_R_WAYPOINT: // Navigate to Waypoint
next_command.lat += home.lat; // offset from home location
next_command.lng += home.lng; // offset from home location
// we've recalculated the WP so we need to set it again
set_next_WP(&next_command);
break;
case CMD_LAND: // LAND to Waypoint
velocity_land = 1000;
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
next_WP.alt = home.alt;
land_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
break;
case CMD_ALTITUDE: // Loiter indefinitely
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
break;
case CMD_LOITER: // Loiter indefinitely
break;
case CMD_LOITER_N_TURNS: // Loiter N Times
break;
case CMD_LOITER_TIME:
break;
case CMD_RTL:
return_to_launch();
break;
}
}
void process_may()
{
//Serial.print("process_may cmd# ");
//Serial.println(wp_index,DEC);
command_may_ID = next_command.id;
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
send_message(SEVERITY_LOW,"New cmd: ");
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
break;
case CMD_ANGLE:
// target angle in degrees
command_yaw_start = nav_yaw; // current position
command_yaw_end = next_command.p1 * 100;
command_yaw_start_time = millis();
// which direction to turn
// 1 = clockwise, -1 = counterclockwise
command_yaw_dir = next_command.lat;
// 1 = Relative or 0 = Absolute
if (next_command.lng == 1) {
// relative
command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
command_yaw_end += nav_yaw;
command_yaw_end = wrap_360(command_yaw_end);
}
// if unspecified go 10° a second
if(command_yaw_speed == 0)
command_yaw_speed = 10;
// if unspecified go clockwise
if(command_yaw_dir == 0)
command_yaw_dir = 1;
// calculate the delta travel
if(command_yaw_dir == 1){
if(command_yaw_start > command_yaw_end){
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
}else{
command_yaw_delta = command_yaw_end - command_yaw_start;
}
}else{
if(command_yaw_start > command_yaw_end){
command_yaw_delta = command_yaw_start - command_yaw_end;
}else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
}
}
command_yaw_delta = wrap_360(command_yaw_delta);
// rate to turn deg per second - default is ten
command_yaw_speed = next_command.alt;
command_yaw_time = command_yaw_delta / command_yaw_speed;
break;
default:
break;
}
}
void process_now()
{
const float t5 = 100000.0;
//Serial.print("process_now cmd# ");
//Serial.println(wp_index,DEC);
byte id = next_command.id;
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
send_message(SEVERITY_LOW, "New cmd: ");
send_message(MSG_COMMAND, wp_index);
// do the command
// --------------
switch(id){
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:
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_ALTITUDE:
if (abs(next_WP.alt - current_loc.alt) < 100){
command_must_index = 0;
}
break;
case CMD_TAKEOFF: // Takeoff!
if (current_loc.alt > (home.alt + takeoff_altitude)){
command_must_index = 0;
takeoff_complete = true;
}
break;
case CMD_LAND:
// 10 - 9 = 1
velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
old_alt = current_loc.alt;
if(velocity_land == 0){
land_complete = true;
command_must_index = 0;
}
break;
case CMD_WAYPOINT: // reach a waypoint
if ((wp_distance > 0) && (wp_distance <= wp_radius)) {
Serial.print("MSG REACHED_WAYPOINT #");
Serial.println(command_must_index,DEC);
// clear the command queue;
command_must_index = 0;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
send_message(SEVERITY_MEDIUM,"Missed WP");
command_must_index = 0;
}
break;
case CMD_LOITER_N_TURNS: // LOITER N times
break;
case CMD_LOITER_TIME: // loiter N milliseconds
break;
case CMD_RTL:
if (wp_distance <= wp_radius) {
//Serial.println("REACHED_HOME");
command_must_index = 0;
}
break;
case CMD_LOITER: // Just plain LOITER
break;
case CMD_ANGLE:
if((millis() - command_yaw_start_time) > command_yaw_time){
command_must_index = 0;
nav_yaw = command_yaw_end;
}
// else we need to be at a certain place
power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
break;
default:
send_message(SEVERITY_HIGH,"No current Must commands");
break;
}
}
void verify_may()
{
switch(command_may_ID) {
case CMD_DELAY:
if ((millis() - delay_start) > delay_timeout){
command_may_index = 0;
delay_timeout = 0;
}
case CMD_LAND_OPTIONS:
break;
}
}

462
ArduCopterMega/config.h Normal file
View File

@ -0,0 +1,462 @@
// -*- 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
//////////////////////////////////////////////////////////////////////////////
// FRAME_CONFIG
//
#ifndef FRAME_CONFIG
# define FRAME_CONFIG PLUS_FRAME
#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)
# define GCS_PORT 3
# else
# define GCS_PORT 0
# endif
#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
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGORIENTATION
# define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_FORWARD
#endif
// run the CLI tool to get this value
#ifndef MAGOFFSET
# define MAGOFFSET 0,0,0
#endif
// Declination is a correction factor between North Pole and real magnetic North. This is different on every location
// IF you want to use really accurate headholding and future navigation features, you should update this
// You can check Declination to your location from http://www.magnetic-declination.com/
#ifndef DECLINATION
# define DECLINATION 0
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE
// FLIGHT_MODE_CHANNEL
//
#ifndef FLIGHT_MODE_CHANNEL
# define FLIGHT_MODE_CHANNEL CH_5
#endif
#if (FLIGHT_MODE_CHANNEL != CH_5) && (FLIGHT_MODE_CHANNEL != CH_6) && (FLIGHT_MODE_CHANNEL != CH_7) && (FLIGHT_MODE_CHANNEL != CH_8)
# error XXX
# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8
# error XXX
#endif
#if !defined(FLIGHT_MODE_1)
# define FLIGHT_MODE_1 STABILIZE
#endif
#if !defined(FLIGHT_MODE_2)
# define FLIGHT_MODE_2 STABILIZE
#endif
#if !defined(FLIGHT_MODE_3)
# define FLIGHT_MODE_3 STABILIZE
#endif
#if !defined(FLIGHT_MODE_4)
# define FLIGHT_MODE_4 STABILIZE
#endif
#if !defined(FLIGHT_MODE_5)
# define FLIGHT_MODE_5 STABILIZE
#endif
#if !defined(FLIGHT_MODE_6)
# define FLIGHT_MODE_6 STABILIZE
#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
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// STARTUP BEHAVIOUR
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GROUND_START_DELAY
//
#ifndef GROUND_START_DELAY
# define GROUND_START_DELAY 0
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// ACRO Rate Control
#ifndef ACRO_RATE_ROLL_P
# define ACRO_RATE_ROLL_P .190
#endif
#ifndef ACRO_RATE_ROLL_I
# define ACRO_RATE_ROLL_I 0.0
#endif
#ifndef ACRO_RATE_ROLL_D
# define ACRO_RATE_ROLL_D 0.0
#endif
#ifndef ACRO_RATE_ROLL_IMAX
# define ACRO_RATE_ROLL_IMAX 20
#endif
#ifndef ACRO_RATE_PITCH_P
# define ACRO_RATE_PITCH_P .190
#endif
#ifndef ACRO_RATE_PITCH_I
# define ACRO_RATE_PITCH_I 0.0
#endif
#ifndef ACRO_RATE_PITCH_D
# define ACRO_RATE_PITCH_D 0.0
#endif
#ifndef ACRO_RATE_PITCH_IMAX
# define ACRO_RATE_PITCH_IMAX 20
#endif
#ifndef ACRO_RATE_YAW_P
# define ACRO_RATE_YAW_P .1
#endif
#ifndef ACRO_RATE_YAW_I
# define ACRO_RATE_YAW_I 0.0
#endif
#ifndef ACRO_RATE_YAW_D
# define ACRO_RATE_YAW_D 0.0
#endif
#ifndef ACRO_RATE_YAW_IMAX
# define ACRO_RATE_YAW_IMAX 20
#endif
// STABILZE RATE Control
//
#ifndef ACRO_RATE_RP
# define ACRO_RATE_RP 0.1
#endif
// STABILZE RATE Control
//
#ifndef ACRO_RATE_YAW
# define ACRO_RATE_YAW 0.1
#endif
//////////////////////////////////////////////////////////////////////////////
// STABILZE Angle Control
//
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 0.44
#endif
#ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.00
#endif
#ifndef STABILIZE_ROLL_D
# define STABILIZE_ROLL_D 0.0
#endif
#ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 3
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 0.44
#endif
#ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.0
#endif
#ifndef STABILIZE_PITCH_D
# define STABILIZE_PITCH_D 0.0
#endif
#ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 3
#endif
// STABILZE RATE Control
//
#ifndef STABILIZE_RATE_RP
# define STABILIZE_RATE_RP 0.1
#endif
//////////////////////////////////////////////////////////////////////////////
// YAW Control
//
#ifndef YAW_P
# define YAW_P 0.3
#endif
#ifndef YAW_I
# define YAW_I 0.0
#endif
#ifndef YAW_D
# define YAW_D 0.0
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 1
#endif
// STABILZE YAW Control
//
#ifndef STABILIZE_RATE_YAW
# define STABILIZE_RATE_YAW 0.008
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control
//
#ifndef THROTTLE_MIN
# define THROTTLE_MIN 0
#endif
#ifndef THROTTLE_CRUISE
# define THROTTLE_CRUISE 250
#endif
#ifndef THROTTLE_MAX
# define THROTTLE_MAX 1000
#endif
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
// how much to we pitch towards the target
#ifndef PITCH_MAX
# define PITCH_MAX 12
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
#ifndef NAV_P
# define NAV_P 2.5
#endif
#ifndef NAV_I
# define NAV_I 0.0 // .01
#endif
#ifndef NAV_D
# define NAV_D 0.0
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 10
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains
//
#ifndef THROTTLE_P
# define THROTTLE_P 0.5
#endif
#ifndef THROTTLE_I
# define THROTTLE_I 0.01
#endif
#ifndef THROTTLE_D
# define THROTTLE_D 0.3
#endif
#ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 50
#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

102
ArduCopterMega/control_modes.pde Normal file
View File

@ -0,0 +1,102 @@
void read_control_switch()
{
byte switchPosition = readSwitch();
//motor_armed = (switchPosition < 5);
if (oldSwitchPosition != switchPosition){
if(motor_armed)
Serial.println("motor_armed");
else
Serial.println("motor disarmed");
set_mode(flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
// reset navigation integrators
// -------------------------
reset_I();
}
}
byte readSwitch(void){
#if FLIGHT_MODE_CHANNEL == CH_5
int pulsewidth = rc_5.radio_in; // default for Arducopter
#elif FLIGHT_MODE_CHANNEL == CH_6
int pulsewidth = rc_6.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_7
int pulsewidth = rc_7.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_8
int pulsewidth = rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
#else
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
#endif
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;
}
void reset_control_switch()
{
oldSwitchPosition = -1;
read_control_switch();
//Serial.print("MSG: reset_control_switch");
//Serial.println(oldSwitchPosition , DEC);
}
void update_servo_switches()
{
}
boolean trim_flag;
unsigned long trim_timer;
// read at 10 hz
// set this to your trainer switch
void read_trim_switch()
{
// switch is engaged
if (rc_7.radio_in > 1500){
if(trim_flag == false){
// called once
trim_timer = millis();
}
trim_flag = true;
//trim_accel();
//Serial.println("trim_accels");
}else{ // switch is disengaged
if(trim_flag){
// switch was just released
if((millis() - trim_timer) > 2000){
/*
if(rc_3.control_in == 0){
imu.init_accel();
imu.print_accel_offsets();
}*/
Serial.printf("r: %d, p: %d\n", rc_1.control_in, rc_2.control_in);
// set new accel offset values
imu.ax(((long)rc_1.control_in * -15) / 100);
imu.ay(((long)rc_2.control_in * -15) / 100);
imu.print_accel_offsets();
} else {
// set the throttle nominal
if(rc_3.control_in > 50){
throttle_cruise = rc_3.control_in;
Serial.print("tnom ");
Serial.println(throttle_cruise, DEC);
save_EEPROM_throttle_cruise();
}
}
trim_flag = false;
}
}
}

41
ArduCopterMega/debug.pde Normal file
View File

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

315
ArduCopterMega/defines.h Normal file
View File

@ -0,0 +1,315 @@
// -*- 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_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 CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_THROTTLE CH_3
#define CH_RUDDER CH_4
#define CH_YAW CH_4
#define CH_AUX CH_5
#define CH_AUX2 CH_6
// motors
#define FRONT 0
#define RIGHT 1
#define BACK 2
#define LEFT 3
#define MAX_SERVO_OUTPUT 2700
#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_NONE -1 // No GCS output
#define PLUS_FRAME 0
#define X_FRAME 1
// Auto Pilot modes
// ----------------
#define ACRO 0 // rate control
#define STABILIZE 1 // hold level position
#define ALT_HOLD 2 // AUTO control
#define AUTO 3 // AUTO control
#define POSITION_HOLD 4 // Hold a single location
#define RTL 5 // AUTO control
#define TAKEOFF 6 // controlled decent rate
#define LAND 7 // controlled decent rate
#define NUM_MODES 8
// 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
#define CMD_ALTITUDE 0x17
#define CMD_R_WAYPOINT 0x18
// 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
#define CMD_ANGLE 0x23 // move servo N to PWM value
// 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
// 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
#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
// sonar
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
// 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
// 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 ToRad(x) (x * 0.01745329252) // *pi/180
//#define ToDeg(x) (x * 57.2957795131) // *180/pi
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
#define PID_SIZE 8
#define RADIO_SIZE 6
// Radio setup
#define EE_RADIO_1 0x00 // all gains stored from here
#define EE_RADIO_2 0x06 // all gains stored from here
#define EE_RADIO_3 0x0C // all gains stored from here
#define EE_RADIO_4 0x12 // all gains stored from here
#define EE_RADIO_5 0x18 // all gains stored from here
#define EE_RADIO_6 0x1E // all gains stored from here
#define EE_RADIO_7 0x24 // all gains stored from here
#define EE_RADIO_8 0x2A // all gains stored from here
// user gains
#define EE_XTRACK_GAIN 0x30
#define EE_XTRACK_ANGLE 0x32
#define EE_PITCH_MAX 0x34
#define EE_DISTANCE_GAIN 0x36
#define EE_ALTITUDE_GAIN 0x38
#define EE_GAIN_1 0x40 // all gains stored from here
#define EE_GAIN_2 0x48 // all gains stored from here
#define EE_GAIN_3 0x50 // all gains stored from here
#define EE_GAIN_4 0x58 // all gains stored from here
#define EE_GAIN_5 0x60 // all gains stored from here
#define EE_GAIN_6 0x68 // all gains stored from here
#define EE_GAIN_7 0x70 // all gains stored from here
#define EE_GAIN_8 0x78 // all gains stored from here
#define EE_GAIN_9 0x80 // all gains stored from here
#define EE_GAIN_10 0x88 // all gains stored from here
#define EE_STAB_RATE_RP 0xA0
#define EE_STAB_RATE_YAW 0xA2
#define EE_ACRO_RATE_RP 0xA4
#define EE_ACRO_RATE_YAW 0xA6
#define EE_MAG_DECLINATION 0xA8
#define EE_MAG_X 0xAA
#define EE_MAG_Y 0xAC
#define EE_MAG_Z 0xAE
#define EE_COMPASS 0xAF
#define EE_FRAME 0xB1
#define EE_IMU_OFFSET 0xE0
//mission specific
#define EE_CONFIG 0X0F8
#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_THROTTLE_MIN 0x103
#define EE_THROTTLE_CRUISE 0x105
#define EE_THROTTLE_MAX 0x107
#define EE_THROTTLE_FAILSAFE 0x10D
#define EE_THROTTLE_FS_VALUE 0x10E
#define EE_THROTTLE_FAILSAFE_ACTION 0x110
#define EE_LOG_BITMASK 0x114
#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)

197
ArduCopterMega/events.pde Normal file
View File

@ -0,0 +1,197 @@
/*
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)
{
}
}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)
{
Serial.print("New Event Loaded ");
Serial.println(cmd->p1,DEC);
if(cmd->p1 == STOP_REPEAT){
Serial.println("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 = rc_5.radio_out;
APM_RC.OutputCh(CH_5, event_value); // send to Servos
undo_event = 2;
break;
case CH_5_TOGGLE:
event_undo_value = rc_6.radio_out;
APM_RC.OutputCh(CH_6, event_value); // send to Servos
undo_event = 2;
break;
case CH_6_TOGGLE:
event_undo_value = rc_7.radio_out;
APM_RC.OutputCh(CH_7, event_value); // send to Servos
undo_event = 2;
break;
case CH_7_TOGGLE:
event_undo_value = rc_8.radio_out;
APM_RC.OutputCh(CH_8, 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();
}
Serial.print("toggle relay ");
Serial.println(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(CH_5, event_undo_value); // send to Servos
break;
case CH_5_TOGGLE:
APM_RC.OutputCh(CH_6, event_undo_value); // send to Servos
break;
case CH_6_TOGGLE:
APM_RC.OutputCh(CH_7, event_undo_value); // send to Servos
break;
case CH_7_TOGGLE:
APM_RC.OutputCh(CH_8, event_undo_value); // send to Servos
break;
case RELAY_TOGGLE:
if(event_undo_value == 1){
relay_A();
}else{
relay_B();
}
Serial.print("untoggle relay ");
Serial.println(PORTL,BIN);
break;
}
}

118
ArduCopterMega/flight_control.pde Normal file
View File

@ -0,0 +1,118 @@
/*************************************************************
throttle control
****************************************************************/
// user input:
// -----------
void output_manual_throttle()
{
rc_3.servo_out = rc_3.control_in;
//rc_3.servo_out = (float)rc_3.servo_out * throttle_boost();
}
// Autopilot
// ---------
void output_auto_throttle()
{
rc_3.servo_out = (float)nav_throttle * throttle_boost();
}
void calc_nav_throttle()
{
long err = constrain (altitude_error, -300, 300); //+-5 meters
long temp = pid_throttle.get_pid(err, deltaMiliSeconds, 1.0);
nav_throttle = (float)(throttle_cruise + temp) * throttle_boost();
}
float throttle_boost()
{
//static byte flipper;
float temp = 1 / (cos(dcm.roll) * cos(dcm.pitch));
if(temp > 1.25)
temp = 1.25;
return temp;
}
/*************************************************************
yaw control
****************************************************************/
void input_yaw_hold()
{
if(rc_3.control_in == 0){
nav_yaw = yaw_sensor;
}else if(rc_4.control_in == 0){
// do nothing
}else{
nav_yaw = yaw_sensor + rc_4.control_in;
nav_yaw = wrap_360(nav_yaw);
}
}
/*void output_yaw_stabilize()
{
rc_4.servo_out = rc_4.control_in;
rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
}*/
/*************************************************************
picth and roll control
****************************************************************/
// how hard to tilt towards the target
// -----------------------------------
void calc_nav_pid()
{
nav_angle = pid_nav.get_pid(wp_distance * 100, dTnav, 1.0);
nav_angle = constrain(nav_angle, -pitch_max, pitch_max);
}
// distribute the pitch angle based on our orientation
// ---------------------------------------------------
void calc_nav_pitch()
{
long b_err = bearing_error;
bool rev = false;
float roll_out;
if(b_err > 18000){
b_err -= 18000;
rev = true;
}
roll_out = abs(b_err - 18000);
roll_out = (9000.0 - roll_out) / 9000.0;
roll_out = (rev) ? roll_out : -roll_out;
nav_pitch = (float)nav_angle * roll_out;
}
// distribute the roll angle based on our orientation
// --------------------------------------------------
void calc_nav_roll()
{
long b_err = bearing_error;
bool rev = false;
float roll_out;
if(b_err > 18000){
b_err -= 18000;
rev = true;
}
roll_out = abs(b_err - 9000);
roll_out = (9000.0 - roll_out) / 9000.0;
roll_out = (rev) ? -roll_out : roll_out;
nav_roll = (float)nav_angle * roll_out;
}

194
ArduCopterMega/navigation.pde Normal file
View File

@ -0,0 +1,194 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
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;
// waypoint distance from plane
// ----------------------------
GPS_wp_distance = getDistance(&current_loc, &next_WP);
if (GPS_wp_distance < 0){
send_message(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return;
}
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrack correction
// -------------------------------------------
nav_bearing = target_bearing;
// look for a big angle change
// ---------------------------
//verify_missed_wp();
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
}
}
void verify_missed_wp()
{
// 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 > 170) loiter_delta -= 360;
if (loiter_delta < -170) loiter_delta += 360;
loiter_sum += abs(loiter_delta);
}
void calc_bearing_error()
{
bearing_error = nav_bearing - yaw_sensor;
bearing_error = wrap_180(bearing_error);
}
void calc_yaw_error()
{
yaw_error = nav_yaw - yaw_sensor;
yaw_error = wrap_180(yaw_error);
}
void calc_distance_error()
{
wp_distance = GPS_wp_distance;
// this wants to work only while moving, but it should filter out jumpy GPS reads
// scale gs to whole deg (50hz / 100) scale bearing error down to whole deg
//distance_estimate += (float)GPS.ground_speed * .0002 * cos(radians(bearing_error / 100));
//distance_estimate -= distance_gain * (float)(distance_estimate - GPS_wp_distance);
//wp_distance = distance_estimate;
}
/*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
} */
// calculated at 50 hz
void calc_altitude_error()
{
altitude_error = next_WP.alt - current_loc.alt;
// 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);
// }
// calc the GPS/Abs pressure altitude
//if(GPS.fix)
// pressure_altitude += altitude_gain * (float)(GPS.altitude - pressure_altitude);
//current_loc.alt = pressure_altitude;
// altitude_error = target_altitude - current_loc.alt;
//Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude);
}
long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
/*
// disabled for now
void update_loiter()
{
loiter_delta = (target_bearing - old_target_bearing) / 100;
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 170) loiter_delta -= 360;
if (loiter_delta < -170) loiter_delta += 360;
loiter_sum += loiter_delta;
} */
void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * x_track_gain, -x_track_angle, x_track_angle);
nav_bearing = wrap_360(nav_bearing);
}
}
void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
int get_altitude_above_home(void)
{
// This is the altitude above the home location
// The GPS gives us altitude at Sea Level
// if you slope soar, you should see a negative number sometimes
// -------------------------------------------------------------
return current_loc.alt - home.alt;
}
long getDistance(struct Location *loc1, struct Location *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
long get_alt_distance(struct Location *loc1, struct Location *loc2)
{
return abs(loc1->alt - loc2->alt);
}
long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}

82
ArduCopterMega/radio.pde Normal file
View File

@ -0,0 +1,82 @@
void init_radio()
{
rc_1.set_angle(4500);
rc_1.dead_zone = 50;
rc_2.set_angle(4500);
rc_2.dead_zone = 50;
rc_3.set_range(0,1000);
rc_3.dead_zone = 20;
rc_3.radio_max += 300; // hack for better throttle control
rc_4.set_angle(6000);
rc_4.dead_zone = 500;
rc_5.set_range(0,1000);
rc_5.set_filter(false);
rc_6.set_range(50,200);
rc_7.set_range(0,1000);
rc_8.set_range(0,1000);
#if ARM_AT_STARTUP == 1
motor_armed = 1;
#endif
APM_RC.OutputCh(CH_1, rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
APM_RC.Init(); // APM Radio initialization
APM_RC.OutputCh(CH_1, rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
}
void read_radio()
{
rc_1.set_pwm(APM_RC.InputCh(CH_1));
rc_2.set_pwm(APM_RC.InputCh(CH_2));
rc_3.set_pwm(APM_RC.InputCh(CH_3));
rc_4.set_pwm(APM_RC.InputCh(CH_4));
rc_5.set_pwm(APM_RC.InputCh(CH_5));
rc_6.set_pwm(APM_RC.InputCh(CH_6));
rc_7.set_pwm(APM_RC.InputCh(CH_7));
rc_8.set_pwm(APM_RC.InputCh(CH_8));
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), rc_1.control_in, rc_2.control_in, rc_3.control_in, rc_4.control_in);
}
void trim_radio()
{
read_radio();
rc_1.trim(); // roll
rc_2.trim(); // pitch
rc_4.trim(); // yaw
}
#define ARM_DELAY 10
#define DISARM_DELAY 10
void arm_motors()
{
static byte arming_counter;
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (rc_3.control_in == 0){
if (rc_4.control_in > 2700) {
if (arming_counter > ARM_DELAY) {
motor_armed = true;
} else{
arming_counter++;
}
}else if (rc_4.control_in < -2700) {
if (arming_counter > DISARM_DELAY){
motor_armed = false;
}else{
arming_counter++;
}
}else{
arming_counter = 0;
}
}
}

115
ArduCopterMega/read_commands.pde Normal file
View File

@ -0,0 +1,115 @@
#define INPUT_BUF_LEN 40
char input_buffer[INPUT_BUF_LEN];
void readCommands(void)
{
static byte bufferPointer = 0;
static byte header[2];
const byte read_GS_header[] = {0x21, 0x21}; //!! Used to verify the payload msg header
if(Serial.available()){
//Serial.println("Serial.available");
byte bufferPointer;
header[0] = Serial.read();
header[1] = Serial.read();
if((header[0] == read_GS_header[0]) && (header[1] == read_GS_header[1])){
// Block until we read full command
// --------------------------------
delay(20);
byte incoming_val = 0;
// Ground Station communication
// ----------------------------
while(Serial.available() > 0)
{
incoming_val = Serial.read();
if (incoming_val != 13 && incoming_val != 10 ) {
input_buffer[bufferPointer++] = incoming_val;
}
if(bufferPointer >= INPUT_BUF_LEN){
Serial.println("Big buffer overrun");
bufferPointer = 0;
input_buffer[0] = 1;
Serial.flush();
memset(input_buffer,0,sizeof(input_buffer));
return;
}
}
parseCommand(input_buffer);
// clear buffer of old data
// ------------------------
memset(input_buffer,0,sizeof(input_buffer));
}else{
Serial.flush();
}
}
}
// Commands can be sent as !!a:100|b:200|c:1
// -----------------------------------------
void parseCommand(char *buffer)
{
Serial.println("got cmd ");
Serial.println(buffer);
char *token, *saveptr1, *saveptr2;
for (int j = 1;; j++, buffer = NULL) {
token = strtok_r(buffer, "|", &saveptr1);
if (token == NULL) break;
char * cmd = strtok_r(token, ":", &saveptr2);
long value = strtol(strtok_r (NULL,":", &saveptr2), NULL,0);
///*
Serial.print("cmd ");
Serial.print(cmd[0]);
Serial.print("\tval ");
Serial.println(value);
Serial.println("");
//*/
///*
switch(cmd[0]){
case 'P':
pid_stabilize_roll.kP((float)value / 1000);
pid_stabilize_pitch.kP((float)value / 1000);
save_EEPROM_PID();
break;
case 'I':
pid_stabilize_roll.kI((float)value / 1000);
pid_stabilize_pitch.kI((float)value / 1000);
save_EEPROM_PID();
break;
case 'D':
pid_stabilize_roll.kD((float)value / 1000);
pid_stabilize_pitch.kD((float)value / 1000);
save_EEPROM_PID();
break;
case 'X':
pid_stabilize_roll.imax((int)(value * 100));
pid_stabilize_pitch.imax((int)(value * 100));
save_EEPROM_PID();
break;
case 'R':
stabilize_rate_roll_pitch = (float)value / 1000;
save_EEPROM_PID();
break;
}
//*/
}
}

48
ArduCopterMega/read_me.text Normal file
View File

@ -0,0 +1,48 @@
Make sure you update the libraries from the arducopter trunk and the latest code from the ardupilotmega branch.
I have a lot of setup commands now. I may reduce and consolidate these soon.
Radio:
ch1 = roll
ch2 = pitch
ch3 = throttle
ch4 = yaw
ch5 = mode switch - use your 3 position switch
ch6 = used for tuning - not currently active
ch7 = use to set throttle hold value while hovering (quick toggle), hold > 5 seconds on ground to reset the accelerometer offsets.
ch8 = not used
setup:
erase = run this first, just in case
reset = run this second
radio = run this third
esc = just ignore this for now
level = optional - sets accelerometer offsets
flat = optional - sets accelerometer offsets to 0
modes = interactive setup for flight modes
pid = optional - writes default PID values to eeprom
frame = optional - default is "+"
enable_mag = enables the compass
disable_mag = disables the compass
compass = interactive setup for compass offsets
declination = usage: "declination 14.25"
show = shows all values
Flight modes to try:
stabilize - I'm having a little trouble inputting Yaw commands, would love some feedback on how to best handle it.
alt_hold - You need to set your throttle_cruise value by toggling ch_7 for a second. Mine is 330
- altitude is controlled by the throttle lever.
position_hold - When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden with the radio.
RTL - will try and fly back to home at the current altitude.
Auto - not tested yet, i'm finishing a waypoint writing sketch and will be ready to test soon
- what's new
- CMD_ANGLE - will set the desired yaw with control of angle/second and direction.
- CMD_ALTITUDE - will send the copter up from current position to desired altitude
- CMD_R_WAYPOINT - is just like a waypoint but relative to home
- CMD_TRACK_WAYPOINT - coming soon, will point the copter at the waypoint no matter the position
Special note:
Any mode other than stabilize will cause the props to spin once the control switch is engaged.
The props will NOT spin in stabilize when throttle is in the off position, even when armed.
Arming is Yaw right for 1 sec, disarm is yaw left for 1 sec. Just give it some juice to confirm arming.
Good luck,
Jason

55
ArduCopterMega/sensors.pde Normal file
View File

@ -0,0 +1,55 @@
void ReadSCP1000(void) {}
void init_pressure_ground(void)
{
for(int i = 0; i < 300; i++){ // We take some readings...
delay(20);
APM_BMP085.Read(); // Get initial data from absolute pressure sensor
abs_pressure_ground = (abs_pressure_ground * 9l + APM_BMP085.Press) / 10l;
ground_temperature = (ground_temperature * 9 + APM_BMP085.Temp) / 10;
}
abs_pressure = APM_BMP085.Press;
}
void read_barometer(void){
float x, scaling, temp;
APM_BMP085.Read(); // Get new data from absolute pressure sensor
//abs_pressure = (abs_pressure + APM_BMP085.Press) >> 1; // Small filtering
abs_pressure = ((float)abs_pressure * .9) + ((float)APM_BMP085.Press * .1); // large filtering
scaling = (float)abs_pressure_ground / (float)abs_pressure;
temp = ((float)ground_temperature / 10.f) + 273.15;
x = log(scaling) * temp * 29271.267f;
current_loc.alt = (long)(x / 10) + home.alt + baro_offset; // Pressure altitude in centimeters
}
// in M/S * 100
void read_airspeed(void)
{
}
#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

708
ArduCopterMega/setup.pde Normal file
View File

@ -0,0 +1,708 @@
// -*- 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_esc (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel_flat (uint8_t argc, const Menu::arg *argv);
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_pid (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass_enable (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass_disable (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},
{"erase", setup_erase},
{"radio", setup_radio},
{"esc", setup_esc},
{"level", setup_accel},
{"flat", setup_accel_flat},
{"modes", setup_flightmodes},
{"pid", setup_pid},
{"frame", setup_frame},
{"enable_mag", setup_compass_enable},
{"disable_mag", setup_compass_disable},
{"compass", setup_compass},
{"declination", setup_declination},
{"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"));
// radio
read_EEPROM_radio();
print_radio_values();
// frame
read_EEPROM_frame();
Serial.printf_P(PSTR("\nFrame type:"));
Serial.printf_P(PSTR("%d \n"), (int)frame_type);
if(frame_type == X_FRAME)
Serial.printf_P(PSTR(" X\n"));
else if(frame_type == PLUS_FRAME)
Serial.printf_P(PSTR("Plus\n"));
read_EEPROM_PID();
Serial.printf_P(PSTR("\nGains:\n"));
// Acro
Serial.printf_P(PSTR("Acro:\nroll:\n"));
print_PID(&pid_acro_rate_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_acro_rate_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_acro_rate_yaw);
Serial.printf_P(PSTR("rates:\n"));
Serial.println(acro_rate_roll_pitch,3);
Serial.printf_P(PSTR("rates_yaw:\n"));
Serial.println(acro_rate_yaw,3);
Serial.println(" ");
// Stabilize
Serial.printf_P(PSTR("Stabilize:\nroll:\n"));
print_PID(&pid_stabilize_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_stabilize_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_yaw);
Serial.printf_P(PSTR("Rate stabilize:\n"));
Serial.println(stabilize_rate_roll_pitch,3);
Serial.println(" ");
// Nav
Serial.printf_P(PSTR("Nav:\npitch:\n"));
print_PID(&pid_nav);
Serial.printf_P(PSTR("throttle:\n"));
print_PID(&pid_throttle);
Serial.println(" ");
// Crosstrack
read_EEPROM_nav();
Serial.printf_P(PSTR("XTRACK:"));
Serial.println(x_track_gain,DEC);
Serial.printf_P(PSTR("XTRACK angle: %d\n"), x_track_angle);
Serial.printf_P(PSTR("PITCH_MAX: %d\n\n"), pitch_max);
// User Configs
read_EEPROM_configs();
Serial.printf_P(PSTR("\nUser config:\n"));
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("log_bitmask: %d\n\n"), log_bitmask);
imu.print_gyro_offsets();
imu.print_accel_offsets();
// mag declination
Serial.printf_P(PSTR("\nCompass "));
if(compass_enabled)
Serial.printf_P(PSTR("en"));
else
Serial.printf_P(PSTR("dis"));
Serial.printf_P(PSTR("abled\n\n"));
// mag declination
read_EEPROM_mag_declination();
Serial.printf_P(PSTR("Mag Delination: "));
Serial.println(mag_declination,2);
// mag offsets
Serial.printf_P(PSTR("Mag offsets: "));
Serial.print(mag_offset_x, 2);
Serial.printf_P(PSTR(", "));
Serial.print(mag_offset_y, 2);
Serial.printf_P(PSTR(", "));
Serial.println(mag_offset_z, 2);
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)
{
/*
saves:
save_EEPROM_waypoint_info();
save_EEPROM_nav();
save_EEPROM_flight_modes();
save_EEPROM_configs();
*/
uint8_t i;
int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort:\n"));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
//Serial.printf_P(PSTR("\nFACTORY RESET\n\n"));
//zero_eeprom();
setup_pid(0 ,NULL);
wp_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius
loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius
save_EEPROM_waypoint_info();
// nav control
x_track_gain = XTRACK_GAIN * 100;
x_track_angle = XTRACK_ENTRY_ANGLE * 100;
pitch_max = PITCH_MAX * 100;
save_EEPROM_nav();
// alt hold
alt_to_hold = -1;
save_EEPROM_alt_RTL();
// default to a + configuration
frame_type = PLUS_FRAME;
save_EEPROM_frame();
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();
// user configs
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;
// 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_EEPROM_configs();
print_done();
// finish
// ------
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(rc_1.radio_in < 500){
while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000);
// stop here
}
}
rc_1.radio_min = rc_1.radio_in;
rc_2.radio_min = rc_2.radio_in;
rc_3.radio_min = rc_3.radio_in;
rc_4.radio_min = rc_4.radio_in;
rc_5.radio_min = rc_5.radio_in;
rc_6.radio_min = rc_6.radio_in;
rc_7.radio_min = rc_7.radio_in;
rc_8.radio_min = rc_8.radio_in;
rc_1.radio_max = rc_1.radio_in;
rc_2.radio_max = rc_2.radio_in;
rc_3.radio_max = rc_3.radio_in;
rc_4.radio_max = rc_4.radio_in;
rc_5.radio_max = rc_5.radio_in;
rc_6.radio_max = rc_6.radio_in;
rc_7.radio_max = rc_7.radio_in;
rc_8.radio_max = rc_8.radio_in;
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: "));
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
rc_1.update_min_max();
rc_2.update_min_max();
rc_3.update_min_max();
rc_4.update_min_max();
rc_5.update_min_max();
rc_6.update_min_max();
rc_7.update_min_max();
rc_8.update_min_max();
if(Serial.available() > 0){
//rc_3.radio_max += 250;
Serial.flush();
save_EEPROM_radio();
//delay(100);
// double checking
//read_EEPROM_radio();
//print_radio_values();
print_done();
break;
}
}
return(0);
}
static int8_t
setup_esc(uint8_t argc, const Menu::arg *argv)
{
Serial.println("\n\nESC Setup - Motors armed");
rc_3.load_eeprom();
rc_1.set_filter(false);
rc_2.set_filter(false);
rc_3.set_filter(false);
rc_4.set_filter(false);
motor_armed = true;
while(1){
delay(20);
read_radio();
APM_RC.OutputCh(CH_1, rc_3.radio_in);
APM_RC.OutputCh(CH_2, rc_3.radio_in);
APM_RC.OutputCh(CH_3, rc_3.radio_in);
APM_RC.OutputCh(CH_4, rc_3.radio_in);
if(Serial.available() > 0){
motor_armed = false;
rc_1.set_filter(true);
rc_2.set_filter(true);
rc_3.set_filter(true);
rc_4.set_filter(true);
read_radio();
Serial.println("\n\nMotors disarmed)");
break;
}
}
return(0);
}
static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
/*
imu.init_gyro();
print_gyro();
imu.load_gyro_eeprom();
print_gyro();
*/
imu.init_accel();
imu.print_accel_offsets();
//imu.load_accel_eeprom();
//print_accel();
return(0);
}
static int8_t
setup_accel_flat(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nClear Accel offsets.\n"));
imu.zero_accel();
imu.print_accel_offsets();
return(0);
}
static int8_t
setup_pid(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nSetting default PID gains\n"));
// acro, angular rate
pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P);
pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
pid_acro_rate_roll.kD(ACRO_RATE_ROLL_D);
pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100);
pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P);
pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I);
pid_acro_rate_pitch.kD(ACRO_RATE_PITCH_D);
pid_acro_rate_pitch.imax(ACRO_RATE_PITCH_IMAX * 100);
pid_acro_rate_yaw.kP(ACRO_RATE_YAW_P);
pid_acro_rate_yaw.kI(ACRO_RATE_YAW_I);
pid_acro_rate_yaw.kD(ACRO_RATE_YAW_D);
pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100);
acro_rate_roll_pitch = ACRO_RATE_RP;
acro_rate_yaw = ACRO_RATE_YAW;
// stabilize, angle error
pid_stabilize_roll.kP(STABILIZE_ROLL_P);
pid_stabilize_roll.kI(STABILIZE_ROLL_I);
pid_stabilize_roll.kD(STABILIZE_ROLL_D);
pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
pid_stabilize_pitch.kP(STABILIZE_PITCH_P);
pid_stabilize_pitch.kI(STABILIZE_PITCH_I);
pid_stabilize_pitch.kD(STABILIZE_PITCH_D);
pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100);
// rate control for angle error
stabilize_rate_roll_pitch = STABILIZE_RATE_RP;
stabilize_rate_yaw = STABILIZE_RATE_YAW;
pid_yaw.kP(YAW_P);
pid_yaw.kI(YAW_I);
pid_yaw.kD(YAW_D);
pid_yaw.imax(YAW_IMAX * 100);
// navigation
pid_nav.kP(NAV_P);
pid_nav.kI(NAV_I);
pid_nav.kD(NAV_D);
pid_nav.imax(NAV_IMAX * 100);
pid_throttle.kP(THROTTLE_P);
pid_throttle.kI(THROTTLE_I);
pid_throttle.kD(THROTTLE_D);
pid_throttle.imax(THROTTLE_IMAX * 100);
save_EEPROM_PID();
print_done();
}
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){
mode = flight_modes[switchPosition];
mode = constrain(mode, 0, NUM_MODES-1);
// update the user
print_switch(switchPosition, mode);
// Remember switch position
oldSwitchPosition = switchPosition;
}
// look for stick input
if (radio_input_switch() == true){
mode++;
if(mode >= NUM_MODES)
mode = 0;
// save new mode
flight_modes[switchPosition] = mode;
// print new mode
print_switch(switchPosition, mode);
}
// escape hatch
if(Serial.available() > 0){
save_EEPROM_flight_modes();
print_done();
return (0);
}
}
}
static int8_t
setup_declination(uint8_t argc, const Menu::arg *argv)
{
mag_declination = argv[1].f;
save_EEPROM_mag_declination();
read_EEPROM_mag_declination();
Serial.printf_P(PSTR("\nsaved: "));
Serial.println(argv[1].f, 2);
}
static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv)
{
zero_eeprom();
return 0;
}
static int8_t
setup_compass_enable(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nCompass enabled:\n"));
compass_enabled = true;
save_EEPROM_mag();
init_compass();
return 0;
}
static int8_t
setup_compass_disable(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nCompass disabled:\n"));
compass_enabled = false;
save_EEPROM_mag();
return 0;
}
static int8_t
setup_frame(uint8_t argc, const Menu::arg *argv)
{
if(argv[1].i < 1){
Serial.printf_P(PSTR("\nUsage:\nPlus frame =>frame 1\nX frame =>frame 2\n\n"));
return 0;
}
if(argv[1].i == 1){
Serial.printf_P(PSTR("\nSaving Plus frame\n\n"));
frame_type = PLUS_FRAME;
}else if(argv[1].i == 2){
Serial.printf_P(PSTR("\nSaving X frame\n\n"));
frame_type = X_FRAME;
}
save_EEPROM_frame();
return 0;
}
static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nRotate/Pitch/Roll your ArduCopter until the offset variables stop changing.\n"));
print_hit_enter();
Serial.printf_P(PSTR("Starting in 3 secs.\n"));
delay(3000);
compass.init(); // Initialization
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
//int counter = 0;
float _min[3], _max[3], _offset[3];
while(1){
static float min[3], _max[3], offset[3];
if (millis() - fast_loopTimer > 100) {
deltaMiliSeconds = millis() - fast_loopTimer;
fast_loopTimer = millis();
G_Dt = (float)deltaMiliSeconds / 1000.f;
compass.read();
compass.calculate(0, 0); // roll = 0, pitch = 0 for this example
// capture min
if(compass.mag_x < _min[0]) _min[0] = compass.mag_x;
if(compass.mag_y < _min[1]) _min[1] = compass.mag_y;
if(compass.mag_z < _min[2]) _min[2] = compass.mag_z;
// capture max
if(compass.mag_x > _max[0]) _max[0] = compass.mag_x;
if(compass.mag_y > _max[1]) _max[1] = compass.mag_y;
if(compass.mag_z > _max[2]) _max[2] = 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.printf_P(PSTR("Heading: "));
Serial.print(ToDeg(compass.heading));
Serial.print(" \t(");
Serial.print(compass.mag_x);
Serial.print(",");
Serial.print(compass.mag_y);
Serial.print(",");
Serial.print(compass.mag_z);
Serial.print(")\t offsets(");
Serial.print(offset[0]);
Serial.print(",");
Serial.print(offset[1]);
Serial.print(",");
Serial.print(offset[2]);
Serial.println(")");
if(Serial.available() > 0){
mag_offset_x = offset[0];
mag_offset_y = offset[1];
mag_offset_z = offset[2];
save_EEPROM_mag_offset();
// set offsets to account for surrounding interference
compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
print_done();
break;
}
}
}
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
void print_PID(PID * pid)
{
Serial.printf_P(PSTR("P: "));
Serial.print((float)pid->kP(),3);
Serial.printf_P(PSTR(",I: "));
Serial.print((float)pid->kI(),3);
Serial.printf_P(PSTR(",D: "));
Serial.print((float)pid->kD(),3);
Serial.printf_P(PSTR(",IMAX: "));
Serial.println(pid->imax()/100,DEC); // imax is stored as *100 degrees internally
}
void
print_radio_values()
{
Serial.printf_P(PSTR("CH1: %d | %d\n"), rc_1.radio_min, rc_1.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d\n"), rc_2.radio_min, rc_2.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d\n"), rc_3.radio_min, rc_3.radio_max);
Serial.printf_P(PSTR("CH4: %d | %d\n"), rc_4.radio_min, rc_4.radio_max);
Serial.printf_P(PSTR("CH5: %d | %d\n"), rc_5.radio_min, rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d\n"), rc_6.radio_min, rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d\n"), rc_7.radio_min, rc_7.radio_max);
Serial.printf_P(PSTR("CH8: %d | %d\n"), rc_8.radio_min, rc_8.radio_max);
}
void
print_switch(byte p, byte m)
{
Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]);
}
void
print_done()
{
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
}
void
print_blanks(int num)
{
while(num > 0){
num--;
Serial.println("");
}
}
// for reading in vales for mode switch
boolean
radio_input_switch(void)
{
static byte bouncer;
if (abs(rc_1.radio_in - rc_1.radio_trim) > 200)
bouncer = 10;
if (bouncer > 0)
bouncer--;
if (bouncer == 1){
return true;
}else{
return false;
}
}
void zero_eeprom(void)
{
byte b;
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b);
}
Serial.printf_P(PSTR("done\n"));
}

407
ArduCopterMega/system.pde Normal file
View File

@ -0,0 +1,407 @@
// -*- 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 1.0.3 Public Alpha\n\n"
#if TELEMETRY_PORT == 3
"Telemetry is on the xbee port\n"
#endif
"freeRAM: %d\n"),freeRAM());
read_EEPROM_startup(); // Read critical config information to start
init_pids();
init_radio();
adc.Init(); // APM ADC library initialization
APM_BMP085.Init(); // APM Abs Pressure sensor initialization
DataFlash.Init(); // DataFlash log initialization
GPS.init(); // GPS Initialization
if(compass_enabled)
init_compass();
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();
//Serial.print("GROUND START");
send_message(SEVERITY_LOW,"GROUND START");
startup_ground();
if (log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// set the correct flight mode
// ---------------------------
reset_control_switch();
}
/*
byte startup_check(void){
if(DEBUG_SUBSYSTEM > 0){
debug_subsystem();
}else{
if (rc_3.radio_in < (rc_3.radio_in + 25)){
// we are on the ground
return 1;
}else{
return 0;
}
}
}
*/
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
void startup_ground(void)
{
/*
read_radio();
while (rc_3.control_in > 0){
delay(20);
read_radio();
APM_RC.OutputCh(CH_1, rc_3.radio_in);
APM_RC.OutputCh(CH_2, rc_3.radio_in);
APM_RC.OutputCh(CH_3, rc_3.radio_in);
APM_RC.OutputCh(CH_4, rc_3.radio_in);
Serial.println("*")
}
*/
if (log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#if(GROUND_START_DELAY > 0)
send_message(SEVERITY_LOW,"With Delay");
delay(GROUND_START_DELAY * 1000);
#endif
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < wp_total + 1; i++) {
send_message(MSG_COMMAND, i);
}
//IMU ground start
//------------------------
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
init_pressure_ground();
#endif
// read the radio to set trims
// ---------------------------
trim_radio();
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro();
// Save the settings for in-air restart
// ------------------------------------
save_EEPROM_groundstart();
// initialize commands
// -------------------
init_commands();
send_message(SEVERITY_LOW,"\n\n Ready to FLY.");
}
void set_mode(byte mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
control_mode = mode;
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
save_EEPROM_PID();
//send_message(SEVERITY_LOW,"control mode");
//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
switch(control_mode)
{
case ACRO:
break;
case STABILIZE:
set_current_loc_here();
break;
case ALT_HOLD:
set_current_loc_here();
break;
case AUTO:
update_auto();
break;
case POSITION_HOLD:
set_current_loc_here();
break;
case RTL:
return_to_launch();
break;
case TAKEOFF:
break;
case LAND:
break;
default:
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 update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
if(GPS.fix == 0){
GPS_light = !GPS_light;
if(GPS_light){
digitalWrite(C_LED_PIN, HIGH);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, HIGH);
}else{
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, LOW);
}
}else{
if(!GPS_light){
GPS_light = true;
digitalWrite(C_LED_PIN, HIGH);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, HIGH);
}
}
}
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();
*/
}
void
init_compass()
{
dcm.set_compass(&compass);
compass.init(false);
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); // set offsets to account for surrounding interference
compass.set_declination(ToRad(mag_declination)); // set local difference between magnetic north and true north
}
/* 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;
}

583
ArduCopterMega/test.pde Normal file
View File

@ -0,0 +1,583 @@
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_flaps(uint8_t argc, const Menu::arg *argv);
static int8_t test_stabilize(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_omega(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_pressure(uint8_t argc, const Menu::arg *argv);
static int8_t test_nav_out(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);
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of printf 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 = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"flaps", test_flaps},
{"stabilize", test_stabilize},
{"gps", test_gps},
{"imu", test_imu},
{"gyro", test_gyro},
{"omega", test_omega},
{"battery", test_battery},
{"relay", test_relay},
{"waypoints", test_wp},
{"airpressure", test_pressure},
{"nav", test_nav_out},
{"compass", test_mag},
{"xbee", test_xbee},
{"eedump", test_eedump},
};
// 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_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), rc_1.radio_in, rc_2.radio_in, rc_3.radio_in, rc_4.radio_in, rc_5.radio_in, rc_6.radio_in, rc_7.radio_in, rc_8.radio_in);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (rc_1.control_in), (rc_2.control_in), (rc_3.control_in), (rc_4.control_in), rc_5.control_in, rc_6.control_in, rc_7.control_in);
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (rc_1.servo_out / 100), (rc_2.servo_out / 100), rc_3.servo_out, (rc_4.servo_out / 100));
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_stabilize(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
//imu.init_gyro();
// read the radio to set trims
// ---------------------------
trim_radio();
control_mode = STABILIZE;
Serial.printf_P(PSTR("pid_stabilize_roll.kP: "));
Serial.println(pid_stabilize_roll.kP(),3);
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
/*
Serial.printf_P(PSTR("pid_yaw.kP: "));
Serial.println(pid_yaw.kP(),3);
Serial.printf_P(PSTR("max_yaw_dampener:%d\n\n "), max_yaw_dampener);
Serial.printf_P(PSTR("stabilize_rate_yaw "));
Serial.print(stabilize_rate_yaw, 3);
Serial.printf_P(PSTR("stabilze_yaw_dampener "));
Serial.print(stabilze_yaw_dampener, 3);
Serial.printf_P(PSTR("\n\n "));
*/
motor_armed = true;
while(1){
// 50 hz
if (millis() - fast_loopTimer > 49) {
deltaMiliSeconds = millis() - fast_loopTimer;
fast_loopTimer = millis();
G_Dt = (float)deltaMiliSeconds / 1000.f;
if(compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
medium_loopCounter = 0;
}
}
// for trim features
read_trim_switch();
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
// IMU
// ---
read_AHRS();
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
//Serial.println(" ");
// write out the servo PWM values
// ------------------------------
set_servos_4();
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
if(Serial.available() > 0){
return (0);
}
}
}
}
static int8_t
test_imu(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("Calibrating."));
imu.init_gyro();
print_hit_enter();
delay(1000);
while(1){
delay(20);
if (millis() - fast_loopTimer > 19) {
deltaMiliSeconds = millis() - fast_loopTimer;
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
read_AHRS();
Vector3f accels = imu.get_accel();
Vector3f gyros = imu.get_gyro();
if(compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
medium_loopCounter = 0;
}
}
// We are using the IMU
// ---------------------
Serial.printf_P(PSTR("A: %d,%d,%d\tG: %d,%d,%d\t"), (int)(accels.x*100), (int)(accels.y*100), (int)(accels.z*100),(int)(gyros.x*100), (int)(gyros.y*100), (int)(gyros.z*100));
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_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(100);
update_GPS();
if(home.lng != 0)
break;
}
while(1){
delay(20);
calc_distance_error();
// 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, spd: %d dist:%d, #sats: %d\n"), (int)GPS.altitude/100, (int)GPS.ground_speed, (int)wp_distance, (int)GPS.num_sats);
}else{
//Serial.print(".");
}
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(1000);
while(1){
Vector3f gyros = imu.get_gyro();
Vector3f accels = imu.get_accel();
Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), (int)gyros.x, (int)gyros.y, (int)gyros.z, (int)accels.x, (int)accels.y, (int)accels.z);
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
/*
static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Gyro | Accel\n"));
delay(1000);
while(1){
Vector3f accels = dcm.get_accel();
Serial.print("accels.z:");
Serial.print(accels.z);
Serial.print("omega.z:");
Serial.print(omega.z);
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
*/
static int8_t
test_omega(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Omega"));
delay(1000);
while(1){
Vector3f omega = dcm.get_gyro();
Serial.printf_P(PSTR("R: %d\tP: %d\tY: %d\n"), (int)(ToDeg(omega.x)), (int)(ToDeg(omega.y)), (int)(ToDeg(omega.z)));
delay(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);
while(1){
Serial.println("Relay A");
relay_A();
delay(3000);
if(Serial.available() > 0){
return (0);
}
Serial.println("Relay B");
relay_B();
delay(3000);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_flaps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(300);
read_radio();
float temp = (float)rc_6.control_in / 1000;
Serial.print("flaps: ");
Serial.println(temp, 3);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
read_EEPROM_waypoint_info();
// save the alitude above home option
if(alt_to_hold == -1){
Serial.printf_P(PSTR("Hold current altitude\n"));
}else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), alt_to_hold/100);
}
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_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;
Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n"));
Serial.printf_P(PSTR("Altitude 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_barometer();
if(i > 200)
sum += abs_pressure;
delay(10);
}
abs_pressure_ground = (float)sum / 100.0;
*/
home.alt = 0;
wp_distance = 0;
init_pressure_ground();
while(1){
if (millis()-fast_loopTimer > 9) {
deltaMiliSeconds = millis() - fast_loopTimer;
G_Dt = (float)deltaMiliSeconds / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
calc_altitude_error();
calc_nav_throttle();
}
if (millis()-medium_loopTimer > 100) {
medium_loopTimer = millis();
read_radio(); // read the radio first
next_WP.alt = home.alt + rc_6.control_in; // 0 - 2000 (20 meters)
read_trim_switch();
read_barometer();
//Serial.printf_P(PSTR("Alt: %dm, Raw: %d\n"), pressure_altitude / 100, abs_pressure); // Someone needs to fix the formatting here for long integers
/*
Serial.print("Altitude: ");
Serial.print((int)current_loc.alt,DEC);
Serial.print("\tnext_alt: ");
Serial.print((int)next_WP.alt,DEC);
Serial.print("\talt_err: ");
Serial.print((int)altitude_error,DEC);
Serial.print("\ttNom: ");
Serial.print(throttle_cruise,DEC);
Serial.print("\ttOut: ");
Serial.println(rc_3.servo_out,DEC);
*/
//Serial.print(" Raw pressure value: ");
//Serial.println(abs_pressure);
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_nav_out(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Nav test\n"));
print_hit_enter();
wp_distance = 100;
dTnav = 50;
while(1){
delay(50);
bearing_error += 100;
bearing_error = wrap_360(bearing_error);
calc_nav_pid();
calc_nav_pitch();
calc_nav_roll();
Serial.printf("error %ld,\troll %ld,\tpitch %ld\n", bearing_error, nav_roll, nav_pitch);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_mag(uint8_t argc, const Menu::arg *argv)
{
if(compass_enabled == false){
Serial.printf_P(PSTR("Compass disabled\n"));
return (0);
}else{
print_hit_enter();
while(1){
delay(250);
compass.read();
compass.calculate(0,0);
Serial.printf_P(PSTR("Heading: ("));
Serial.print(ToDeg(compass.heading));
Serial.printf_P(PSTR(") XYZ: ("));
Serial.print(compass.mag_x);
Serial.print(comma);
Serial.print(compass.mag_y);
Serial.print(comma);
Serial.print(compass.mag_z);
Serial.println(")");
if(Serial.available() > 0){
return (0);
}
}
}
}
void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
}