diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h new file mode 100644 index 0000000000..0facb1e8ce --- /dev/null +++ b/ArduCopterMega/APM_Config.h @@ -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 + + diff --git a/ArduCopterMega/APM_Config.h.reference b/ArduCopterMega/APM_Config.h.reference new file mode 100644 index 0000000000..5d575ac328 --- /dev/null +++ b/ArduCopterMega/APM_Config.h.reference @@ -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. diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde new file mode 100644 index 0000000000..da1399054b --- /dev/null +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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 +#include +#include +#include + +// Libraries +#include +#include +#include // ArduPilot Mega RC Library +#include // ArduPilot Mega RC Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot GPS library +#include // Arduino I2C lib +#include // ArduPilot Mega BMP085 Library +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega IMU Library +#include // ArduPilot Mega DCM Library +#include // 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; +} \ No newline at end of file diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde new file mode 100644 index 0000000000..39c28409c6 --- /dev/null +++ b/ArduCopterMega/Attitude.pde @@ -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 +} + diff --git a/ArduCopterMega/EEPROM map.txt b/ArduCopterMega/EEPROM map.txt new file mode 100644 index 0000000000..cd82212aa4 --- /dev/null +++ b/ArduCopterMega/EEPROM map.txt @@ -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 .. + + + \ No newline at end of file diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde new file mode 100644 index 0000000000..7e51b9f13c --- /dev/null +++ b/ArduCopterMega/EEPROM.pde @@ -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); +} diff --git a/ArduCopterMega/GCS.pde b/ArduCopterMega/GCS.pde new file mode 100644 index 0000000000..cbec6d2d19 --- /dev/null +++ b/ArduCopterMega/GCS.pde @@ -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; +} diff --git a/ArduCopterMega/GCS_Ardupilot.pde b/ArduCopterMega/GCS_Ardupilot.pde new file mode 100644 index 0000000000..e94db2993e --- /dev/null +++ b/ArduCopterMega/GCS_Ardupilot.pde @@ -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 diff --git a/ArduCopterMega/GCS_IMU_ouput.pde b/ArduCopterMega/GCS_IMU_ouput.pde new file mode 100644 index 0000000000..9456a4b16c --- /dev/null +++ b/ArduCopterMega/GCS_IMU_ouput.pde @@ -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 diff --git a/ArduCopterMega/GCS_Jason_text.pde b/ArduCopterMega/GCS_Jason_text.pde new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ArduCopterMega/GCS_Standard.pde b/ArduCopterMega/GCS_Standard.pde new file mode 100644 index 0000000000..233c089681 --- /dev/null +++ b/ArduCopterMega/GCS_Standard.pde @@ -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> 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 diff --git a/ArduCopterMega/GCS_Xplane.pde b/ArduCopterMega/GCS_Xplane.pde new file mode 100644 index 0000000000..e6f4c2f235 --- /dev/null +++ b/ArduCopterMega/GCS_Xplane.pde @@ -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> 8) & 0xff; +} + +void output_byte(byte value) +{ + out_buffer[buf_len++] = value; +} + +void print_buffer() +{ + byte ck_a = 0; + byte ck_b = 0; + for (byte i = 0;i < buf_len; i++){ + Serial.print (out_buffer[i]); + } + Serial.print('\r'); + Serial.print('\n'); + buf_len = 0; +} + + +#endif + diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde new file mode 100644 index 0000000000..cd54352d68 --- /dev/null +++ b/ArduCopterMega/Log.pde @@ -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 dump log \n" + " erase erase all logs\n" + " enable |all enable logging or everything\n" + " disable |all disable logging or everything\n" + "\n")); +} + +// Creates a constant array of structs representing menu options +// and stores them in Flash memory, not RAM. +// User enters the string in the console to call the functions on the right. +// See class Menu in AP_Coommon for implementation details +const struct Menu::command log_menu_commands[] PROGMEM = { + {"dump", dump_log}, + {"erase", erase_logs}, + {"enable", select_logs}, + {"disable", select_logs}, + {"help", help_log} +}; + +// A Macro to create the Menu +MENU2(log_menu, "Log", log_menu_commands, print_log_menu); + +static bool +print_log_menu(void) +{ + int log_start; + int log_end; + byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); + + Serial.printf_P(PSTR("logs enabled: ")); + if (0 == log_bitmask) { + Serial.printf_P(PSTR("none")); + } else { + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // the bit being set and print the name of the log option to suit. +#define PLOG(_s) if (log_bitmask & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) + PLOG(ATTITUDE_FAST); + PLOG(ATTITUDE_MED); + PLOG(GPS); + PLOG(PM); + PLOG(CTUN); + PLOG(NTUN); + PLOG(MODE); + PLOG(RAW); + PLOG(CMD); +#undef PLOG + } + Serial.println(); + + if (last_log_num == 0) { + Serial.printf_P(PSTR("\nNo logs available for download\n")); + } else { + + Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); + for(int i=1;i last_log_num)) { + Serial.printf_P(PSTR("bad log number\n")); + return(-1); + } + + dump_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log-1)*0x02)); + dump_log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log)*0x02))-1; + if (dump_log == last_log_num) { + dump_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); + } + Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), + dump_log, dump_log_start, dump_log_end); + Log_Read(dump_log_start, dump_log_end); + Serial.printf_P(PSTR("Log read complete\n")); +} + +static int8_t +erase_logs(uint8_t argc, const Menu::arg *argv) +{ + for(int i = 10 ; i > 0; i--) { + Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); + delay(1000); + } + Serial.printf_P(PSTR("\nErasing log...\n")); + for(int j = 1; j < 4001; j++) + DataFlash.PageErase(j); + eeprom_write_byte((uint8_t *)EE_LAST_LOG_NUM, 0); + eeprom_write_byte((uint8_t *)EE_LAST_LOG_PAGE, 1); + Serial.printf_P(PSTR("\nLog erased.\n")); +} + +static int8_t +select_logs(uint8_t argc, const Menu::arg *argv) +{ + uint16_t bits; + + if (argc != 2) { + Serial.printf_P(PSTR("missing log type\n")); + return(-1); + } + + bits = 0; + + // Macro to make the following code a bit easier on the eye. + // Pass it the capitalised name of the log option, as defined + // in defines.h but without the LOG_ prefix. It will check for + // that name as the argument to the command, and set the bit in + // bits accordingly. + // + if (!strcasecmp_P(argv[1].str, PSTR("all"))) { + bits = ~(bits = 0); + } else { +#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s + TARG(ATTITUDE_FAST); + TARG(ATTITUDE_MED); + TARG(GPS); + TARG(PM); + TARG(CTUN); + TARG(NTUN); + TARG(MODE); + TARG(RAW); + TARG(CMD); +#undef TARG + } + + if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { + log_bitmask |= bits; + } else { + log_bitmask &= ~bits; + } + save_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; i7) logvar = DataFlash.ReadInt(); + else logvar = DataFlash.ReadByte(); + //if(y > 7) logvar = logvar/1000.f; + Serial.print(logvar); + Serial.print(comma); + } + Serial.println(" "); +} + +// Read a command processing packet +void Log_Read_Cmd() +{ + byte logvarb; + long logvarl; + + Serial.print("CMD:"); + for(int i=1;i<4;i++) { + logvarb = DataFlash.ReadByte(); + Serial.print(logvarb,DEC); + Serial.print(comma); + } + for(int i=1;i<4;i++) { + logvarl = DataFlash.ReadLong(); + Serial.print(logvarl,DEC); + Serial.print(comma); + } + Serial.println(" "); +} + +void Log_Read_Startup() +{ + byte logbyte = DataFlash.ReadByte(); + if (logbyte == TYPE_AIRSTART_MSG) + Serial.print("AIR START - "); + else if (logbyte == TYPE_GROUNDSTART_MSG) + Serial.print("GROUND START - "); + else + Serial.print("UNKNOWN STARTUP TYPE -"); + Serial.print(DataFlash.ReadByte(), DEC); + Serial.println(" commands in memory"); +} + +// Read an attitude packet +void Log_Read_Attitude() +{ + int log_roll; + int log_pitch; + uint16_t log_yaw; + + log_roll = DataFlash.ReadInt(); + log_pitch = DataFlash.ReadInt(); + log_yaw = (uint16_t)DataFlash.ReadInt(); + + Serial.print("ATT:"); + Serial.print(log_roll); + Serial.print(comma); + Serial.print(log_pitch); + Serial.print(comma); + Serial.print(log_yaw); + Serial.println(); +} + +// Read a mode packet +void Log_Read_Mode() +{ + byte mode; + + mode = DataFlash.ReadByte(); + Serial.print("MOD:"); + 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); +} + + diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt new file mode 100644 index 0000000000..0cf653c2b3 --- /dev/null +++ b/ArduCopterMega/command description.txt @@ -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 + + + + diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde new file mode 100644 index 0000000000..57e1ac3995 --- /dev/null +++ b/ArduCopterMega/commands.pde @@ -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(¤t_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(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + + print_current_waypoints(); + + target_bearing = get_bearing(¤t_loc, &next_WP); + old_target_bearing = target_bearing; + // this is used to offset the shrinking longitude as we go towards the poles + + // set a new crosstrack bearing + // ---------------------------- + reset_crosstrack(); +} + + +// run this at setup on the ground +// ------------------------------- +void init_home() +{ + 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; +} + + + diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde new file mode 100644 index 0000000000..f4deb0e68c --- /dev/null +++ b/ArduCopterMega/commands_process.pde @@ -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; + } +} + diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h new file mode 100644 index 0000000000..123951c64c --- /dev/null +++ b/ArduCopterMega/config.h @@ -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 + + + diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde new file mode 100644 index 0000000000..5688885075 --- /dev/null +++ b/ArduCopterMega/control_modes.pde @@ -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; + } + } +} \ No newline at end of file diff --git a/ArduCopterMega/debug.pde b/ArduCopterMega/debug.pde new file mode 100644 index 0000000000..ccb6f63d3a --- /dev/null +++ b/ArduCopterMega/debug.pde @@ -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 + diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h new file mode 100644 index 0000000000..4ffba84d2b --- /dev/null +++ b/ArduCopterMega/defines.h @@ -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) + diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde new file mode 100644 index 0000000000..b42c160306 --- /dev/null +++ b/ArduCopterMega/events.pde @@ -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; + } +} diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde new file mode 100644 index 0000000000..50d3abb64b --- /dev/null +++ b/ArduCopterMega/flight_control.pde @@ -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; +} + diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde new file mode 100644 index 0000000000..a6a7689326 --- /dev/null +++ b/ArduCopterMega/navigation.pde @@ -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(¤t_loc, &next_WP); + + if (GPS_wp_distance < 0){ + send_message(SEVERITY_HIGH," 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(¤t_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(¤t_loc, &next_WP); // Used for track following +} + +int get_altitude_above_home(void) +{ + // This is the altitude above the home location + // The GPS gives us altitude at Sea Level + // if you slope soar, you should see a negative number sometimes + // ------------------------------------------------------------- + return current_loc.alt - home.alt; +} + +long getDistance(struct Location *loc1, struct Location *loc2) +{ + if(loc1->lat == 0 || loc1->lng == 0) + return -1; + if(loc2->lat == 0 || loc2->lng == 0) + return -1; + float dlat = (float)(loc2->lat - loc1->lat); + float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; + return sqrt(sq(dlat) + sq(dlong)) * .01113195; +} + +long get_alt_distance(struct Location *loc1, struct Location *loc2) +{ + return abs(loc1->alt - loc2->alt); +} + +long get_bearing(struct Location *loc1, struct Location *loc2) +{ + long off_x = loc2->lng - loc1->lng; + long off_y = (loc2->lat - loc1->lat) * scaleLongUp; + long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795; + if (bearing < 0) bearing += 36000; + return bearing; +} diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde new file mode 100644 index 0000000000..5a97611021 --- /dev/null +++ b/ArduCopterMega/radio.pde @@ -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; + } + } +} diff --git a/ArduCopterMega/read_commands.pde b/ArduCopterMega/read_commands.pde new file mode 100644 index 0000000000..728ec4c9f1 --- /dev/null +++ b/ArduCopterMega/read_commands.pde @@ -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; + } + //*/ + } +} + + + + + diff --git a/ArduCopterMega/read_me.text b/ArduCopterMega/read_me.text new file mode 100644 index 0000000000..89dbfbcf5e --- /dev/null +++ b/ArduCopterMega/read_me.text @@ -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 + diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde new file mode 100644 index 0000000000..f2afa4469d --- /dev/null +++ b/ArduCopterMega/sensors.pde @@ -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 + diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde new file mode 100644 index 0000000000..3a22052aff --- /dev/null +++ b/ArduCopterMega/setup.pde @@ -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")); +} diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde new file mode 100644 index 0000000000..1ec2369783 --- /dev/null +++ b/ArduCopterMega/system.pde @@ -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; +} + diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde new file mode 100644 index 0000000000..adb4880a7e --- /dev/null +++ b/ArduCopterMega/test.pde @@ -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")); +}