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

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

This commit is contained in:
jasonshort 2011-02-24 05:56:59 +00:00
parent 767fcf76bb
commit 2555abcf1d
23 changed files with 903 additions and 896 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
ArduCopterMega/APM_Config.h.reference
View File

@ -118,7 +118,7 @@
//#define SERIAL0_BAUD 38400
//#define SERIAL3_BAUD 115200
//
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring OPTIONAL
//
@ -127,7 +127,7 @@
//
// BATTERY_EVENT OPTIONAL
//
// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is
// Set BATTERY_EVENT to ENABLED to enable battery monitoring. The default is
// DISABLED.
//
// BATTERY_TYPE OPTIONAL if BATTERY_EVENT is set
@ -184,16 +184,16 @@
// 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
// 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.
// 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)
// ------------+---------------------
@ -259,7 +259,7 @@
// 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
// 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
@ -273,15 +273,15 @@
// 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
// 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
// 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,
// 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.
//
@ -309,16 +309,16 @@
// 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
// 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.
//
@ -351,7 +351,7 @@
// 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
// in slower error correction and smoother output. The default is a
// reasonable starting point.
//
//#define ALT_EST_GAIN 0.01
@ -372,7 +372,7 @@
//
// THROTTLE_MIN OPTIONAL
//
// The minimum throttle setting to which the autopilot will reduce the
// 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
@ -386,7 +386,7 @@
// 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
// Reduce this value if your aicraft is overpowered, or has complex flight
// characteristics at high throttle settings.
//
//#define THROTTLE_MIN 0
@ -419,7 +419,7 @@
// 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
// Note: When tuning these values, start with changes of no more than 25% at
// a time.
//
// SERVO_ROLL_P OPTIONAL
@ -473,9 +473,9 @@
//
// RUDDER_MIX OPTIONAL
//
// Roll to yaw mixing. This allows for co-ordinated turns.
// 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
@ -495,7 +495,7 @@
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
//
// Tuning values for the navigation control PID loops.
//
// The P term is the primary tuning value. This determines how the control
@ -503,17 +503,17 @@
//
// The I term is used to control drift.
//
// The D term is used to control overshoot. Avoid adjusting this term if
// 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
// 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
// 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.
@ -544,7 +544,7 @@
//
// 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
@ -577,7 +577,7 @@
//
// The I term is used to compensate for small offsets.
//
// The D term is used to control overshoot. Avoid adjusting this term if
// 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
@ -609,7 +609,7 @@
//#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_TE_INT_MAX 20
//#define THROTTLE_SLEW_LIMIT 0
//#define P_TO_T 2.5
//
@ -638,36 +638,6 @@
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// 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
//
@ -690,12 +660,12 @@
//
// LOG_ATTITUDE_FAST DEBUG
//
// Logs basic attitude info to the dataflash at 50Hz (uses more space).
// 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
// Logs basic attitude info to the dataflash at 10Hz (uses less space than
// LOG_ATTITUDE_FAST). Defaults to ENABLED.
//
// LOG_GPS OPTIONAL
@ -704,7 +674,7 @@
//
// LOG_PM OPTIONAL
//
// Logs IMU performance monitoring info every 20 seconds.
// Logs IMU performance monitoring info every 20 seconds.
// Defaults to DISABLED.
//
// LOG_CTUN OPTIONAL
@ -728,7 +698,7 @@
//
// LOG_CMD OPTIONAL
//
// Logs new commands when they process.
// Logs new commands when they process.
// Defaults to ENABLED.
//
//#define LOG_ATTITUDE_FAST DISABLED
@ -743,7 +713,7 @@
//
//
// Do not remove - this is to discourage users from copying this file
// 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.

52
ArduCopterMega/APM_Config_mavlink_hil.h Normal file
View File

@ -0,0 +1,52 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Hardware in the loop protocol
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
// HIL_MODE SELECTION
//
// Mavlink supports
// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
// 2. HIL_MODE_SENSORS: full sensor simulation
#define HIL_MODE HIL_MODE_ATTITUDE
// HIL_PORT SELCTION
//
// PORT 1
// If you would like to run telemetry communications for a groundstation
// while you are running hardware in the loop it is necessary to set
// HIL_PORT to 1. This uses the port that would have been used for the gps
// as the hardware in the loop port. You will have to solder
// headers onto the gps port connection on the apm
// and connect via an ftdi cable.
//
// The baud rate is set to 115200 in this mode.
//
// PORT 3
// If you don't require telemetry communication with a gcs while running
// hardware in the loop you may use the telemetry port as the hardware in
// the loop port. Alternatively, use a telemetry/HIL shim like FGShim
// https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear
//
// The buad rate is controlled by SERIAL3_BAUD in this mode.
#define HIL_PORT 3
// You can set your gps protocol here for your actual
// hardware and leave it without affecting the hardware
// in the loop simulation
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
// Ground control station comms
#if HIL_PORT != 3
# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
# define GCS_PORT 3
#endif
// Sensors
// All sensors are supported in all modes.
// The magnetometer is not used in
// HIL_MODE_ATTITUDE but you may leave it
// enabled if you wish.
#define AIRSPEED_SENSOR ENABLED
#define MAGNETOMETER ENABLED

14
ArduCopterMega/APM_Config_xplane.h Normal file
View File

@ -0,0 +1,14 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PORT 3
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED
#define AIRSPEED_SENSOR ENABLED

110
ArduCopterMega/ArduCopterMega.pde
View File

@ -26,17 +26,18 @@ version 2.1 of the License, or (at your option) any later version.
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <APM_BinComm.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // ArduPilot Mega RC Library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
@ -50,6 +51,7 @@ version 2.1 of the License, or (at your option) any later version.
#include "Parameters.h"
#include "global_data.h"
#include "GCS.h"
#include "HIL.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
@ -87,33 +89,63 @@ Parameters g;
// All GPS access should be through this pointer.
GPS *g_gps;
#if HIL_MODE == HIL_MODE_NONE
// real sensors
AP_ADC_ADS7844 adc;
APM_BMP085_Class barometer;
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
AP_ADC_ADS7844 adc;
APM_BMP085_Class barometer;
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver(&Serial1);
AP_GPS_NMEA g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver(&Serial1);
AP_GPS_SIRF g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver(&Serial1);
AP_GPS_UBLOX g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver(&Serial1);
AP_GPS_MTK g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
AP_GPS_MTK16 g_gps_driver(&Serial1);
AP_GPS_MTK16 g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver(NULL);
AP_GPS_None g_gps_driver(NULL);
#else
#error Unrecognised GPS_PROTOCOL setting.
#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL
#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
AP_ADC_HIL adc;
APM_BMP085_HIL_Class barometer;
AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL);
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); // normal imu
AP_DCM dcm(&imu, g_gps);
#elif HIL_MODE == HIL_MODE_ATTITUDE
AP_DCM_HIL dcm;
AP_GPS_HIL g_gps_driver(NULL);
#else
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
GCS_MAVLINK hil;
#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
HIL_XPLANE hil;
#endif // HIL PROTOCOL
#endif // HIL_MODE
#if HIL_MODE != HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_SENSORS
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); // normal imu
#else
AP_IMU_Shim imu; // hil imu
#endif
AP_DCM dcm(&imu, g_gps); // normal dcm
#endif
////////////////////////////////////////////////////////////////////////////////
// GCS selection
@ -306,7 +338,7 @@ byte command_yaw_dir;
// Waypoints
// ---------
long GPS_wp_distance; // meters - distance between plane and next waypoint
//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
@ -369,11 +401,12 @@ char display_PID = -1; // Flag used by DebugTerminal to indicate that the ne
// --------------
unsigned long fast_loopTimer; // Time in miliseconds of main control loop
unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
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;
uint8_t delta_ms_medium_loop;
byte slow_loopCounter;
byte superslow_loopCounter;
@ -382,8 +415,6 @@ byte fbw_timer; // for limiting the execution of FBW input
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav
uint8_t delta_ms_medium_loop;
uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
unsigned long dTnav2; // Delta Time in milliseconds for navigation computations
@ -424,9 +455,9 @@ void loop()
if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT);
if (g.log_bitmask & MASK_LOG_PM){
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
}
resetPerfData();
}
}
@ -488,7 +519,7 @@ void medium_loop()
medium_loopCounter++;
if(g_gps->new_data){
g_gps->new_data = false;
g_gps->new_data = false;
dTnav = millis() - nav_loopTimer;
nav_loopTimer = millis();
@ -576,7 +607,7 @@ void medium_loop()
calc_bearing_error();
// guess how close we are - fixed observer calc
calc_distance_error();
//calc_distance_error();
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
@ -605,14 +636,18 @@ void slow_loop()
slow_loopCounter++;
superslow_loopCounter++;
if(superslow_loopCounter >= 200){
if(superslow_loopCounter >= 200){ // Execute every minute
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled) {
compass.save_offsets();
}
#endif
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
if(g.compass_enabled){
compass.save_offsets();
}
superslow_loopCounter = 0;
}
}
break;
case 1:
@ -683,7 +718,7 @@ void update_GPS(void)
SendDebugln("!! bad loc");
ground_start_count = 5;
} else {
}else{
//Serial.printf("init Home!");
if (g.log_bitmask & MASK_LOG_CMD)
@ -787,6 +822,7 @@ void update_current_flight_mode(void)
}
break;
case LOITER:
case STABILIZE:
// clear any AP naviagtion values
nav_pitch = 0;

4
ArduCopterMega/Attitude.pde
View File

@ -217,7 +217,7 @@ void calc_nav_throttle()
g.pid_baro_throttle.kP(t);
} else {
}else{
// SONAR
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
@ -245,7 +245,7 @@ void output_manual_yaw()
{
if(g.rc_3.control_in == 0){
clear_yaw_control();
} else {
}else{
// Yaw control
if(g.rc_4.control_in == 0){
//clear_yaw_control();

6
ArduCopterMega/GCS_Mavlink.pde
View File

@ -16,7 +16,7 @@ GCS_MAVLINK::init(BetterStream * port)
if (port == &Serial) { // to split hil vs gcs
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
} else {
}else{
mavlink_comm_1_port = port;
chan = MAVLINK_COMM_1;
}
@ -347,7 +347,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
x = tell_command.lat/1.0e7; // local (x), global (longitude)
y = tell_command.lng/1.0e7; // local (y), global (latitude)
z = tell_command.alt/1.0e2; // local (z), global (altitude)
} else {
}else{
// command is raw
x = tell_command.lat;
y = tell_command.lng;
@ -508,7 +508,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
tell_command.lat = packet.x; // in as long no conversion
tell_command.lng = packet.y; // in as long no conversion
tell_command.alt = packet.z; // in as int no conversion
} else {
}else{
tell_command.p1 = packet.param1; // in as byte no conversion
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7

40
ArduCopterMega/Log.pde
View File

@ -54,7 +54,7 @@ print_log_menu(void)
Serial.printf_P(PSTR("logs enabled: "));
if (0 == g.log_bitmask) {
Serial.printf_P(PSTR("none"));
} else {
}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
@ -76,7 +76,7 @@ print_log_menu(void)
if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs available for download\n"));
} else {
}else{
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
for(int i=1;i<last_log_num+1;i++) {
@ -151,7 +151,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
//
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~(bits = 0);
} else {
}else{
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s
TARG(ATTITUDE_FAST);
TARG(ATTITUDE_MED);
@ -168,7 +168,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
g.log_bitmask.set_and_save(g.log_bitmask | bits);
} else {
}else{
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
}
return(0);
@ -206,7 +206,7 @@ byte get_num_logs(void)
if(data==LOG_INDEX_MSG){
byte num_logs = DataFlash.ReadByte();
return num_logs;
} else {
}else{
log_step=0; // Restart, we have a problem...
}
break;
@ -248,7 +248,7 @@ void start_new_log(byte num_existing_logs)
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
} else {
}else{
gcs.send_text(SEVERITY_LOW,"<start_new_log> Logs full - logging discontinued");
}
}
@ -285,7 +285,7 @@ void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end
end_page = find_last_log_page(start_page);
return; // This is the normal exit point
} else {
}else{
log_step=0; // Restart, we have a problem...
}
break;
@ -381,6 +381,7 @@ void Log_Write_Startup(byte type)
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Control_Tuning()
{
Vector3f accel = imu.get_accel();
@ -396,9 +397,10 @@ void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((int)(g.rc_3.servo_out));
DataFlash.WriteInt((int)(g.rc_4.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteByte(END_BYTE);
}
#endif
// Write a navigation tuning packet. Total length : 18 bytes
void Log_Write_Nav_Tuning()
@ -447,6 +449,7 @@ void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long
}
// Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Raw()
{
Vector3f gyro = imu.get_gyro();
@ -463,9 +466,10 @@ void Log_Write_Raw()
DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z);
DataFlash.WriteByte(END_BYTE);
}
#endif
void Log_Write_Current()
{
@ -482,7 +486,7 @@ void Log_Write_Current()
// Read a Current packet
void Log_Read_Current()
{
Serial.printf("CURR: %d, %4.4f, %4.4f, %d\n",
Serial.printf("CURR: %d, %4.4f, %4.4f, %d\n",
DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
@ -519,10 +523,10 @@ void Log_Read_Nav_Tuning()
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/100.0); // Altitude error
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/100.0); // Airspeed
Serial.print((float)DataFlash.ReadInt()/100.0); // Airspeed
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/1000.0); // nav_gain_scaler
Serial.println(comma);
//Serial.print((float)DataFlash.ReadInt()/1000.0); // nav_gain_scaler
//Serial.println(comma);
}
// Read a performance packet
@ -600,7 +604,7 @@ void Log_Read_Attitude()
// Read a mode packet
void Log_Read_Mode()
{
{
Serial.print("MOD:");
Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
}
@ -696,11 +700,11 @@ void Log_Read(int start_page, int end_page)
}else if(data==LOG_CMD_MSG){
Log_Read_Cmd();
log_step++;
}else if(data==LOG_CURRENT_MSG){
Log_Read_Current();
log_step++;
}else if(data==LOG_STARTUP_MSG){
Log_Read_Startup();
log_step++;
@ -708,9 +712,9 @@ void Log_Read(int start_page, int end_page)
if(data == LOG_GPS_MSG){
Log_Read_GPS();
log_step++;
} else {
}else{
Serial.print("Error Reading Packet: ");
Serial.print(packet_count);
Serial.print(packet_count);
log_step = 0; // Restart, we have a problem...
}
}

64
ArduCopterMega/Mavlink_Common.h
View File

@ -20,52 +20,6 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
else return 0; // no error
}
/**
* @brief Send low-priority messages at a maximum rate of xx Hertz
*
* This function sends messages at a lower rate to not exceed the wireless
* bandwidth. It sends one message each time it is called until the buffer is empty.
* Call this function with xx Hertz to increase/decrease the bandwidth.
*/
static void mavlink_queued_send(mavlink_channel_t chan)
{
AP_Var *vp;
float value;
// send parameters one by one and prevent cross port comms
if (NULL != global_data.parameter_p && global_data.requested_interface == chan) {
// if the value can't be represented as a float, we will skip it here
vp = global_data.parameter_p;
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send(chan,
(int8_t*)param_name,
value,
256,
vp->meta_get_handle());
}
// remember the next variable we're going to send
global_data.parameter_p = vp->next();
}
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
mavdelay++;
// request waypoints one by one
if (global_data.waypoint_receiving && global_data.requested_interface == chan &&
global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz
{
mavlink_msg_waypoint_request_send(chan,
global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid ,global_data.waypoint_request_i);
mavdelay = 0;
}
}
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
{
@ -137,8 +91,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(chan,current_loc.lat,
current_loc.lng,current_loc.alt*10,g_gps.ground_speed/1.0e2*rot.a.x,
g_gps.ground_speed/1.0e2*rot.b.x,g_gps.ground_speed/1.0e2*rot.c.x);
current_loc.lng,current_loc.alt*10,g_gps->ground_speed/1.0e2*rot.a.x,
g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
break;
}
case MSG_LOCAL_LOCATION:
@ -146,15 +100,15 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth,
ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)),
(current_loc.alt-home.alt)/1.0e2, g_gps.ground_speed/1.0e2*rot.a.x,
g_gps.ground_speed/1.0e2*rot.b.x,g_gps.ground_speed/1.0e2*rot.c.x);
(current_loc.alt-home.alt)/1.0e2, g_gps->ground_speed/1.0e2*rot.a.x,
g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
break;
}
case MSG_GPS_RAW:
{
mavlink_msg_gps_raw_send(chan,timeStamp,g_gps.status(),
g_gps.latitude/1.0e7,g_gps.longitude/1.0e7,g_gps.altitude/100.0,
g_gps.hdop,0.0,g_gps.ground_speed/100.0,g_gps.ground_course/100.0);
mavlink_msg_gps_raw_send(chan,timeStamp,g_gps->status(),
g_gps->latitude/1.0e7,g_gps->longitude/1.0e7,g_gps->altitude/100.0,
g_gps->hdop,0.0,g_gps->ground_speed/100.0,g_gps->ground_course/100.0);
break;
}
case MSG_SERVO_OUT:
@ -200,7 +154,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
}
case MSG_VFR_HUD:
{
mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)g_gps.ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)g_gps->ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
climb_rate, (int)rc[CH_THROTTLE]->servo_out);
break;
}
@ -224,7 +178,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_GPS_STATUS:
{
mavlink_msg_gps_status_send(chan,g_gps.num_sats,NULL,NULL,NULL,NULL,NULL);
mavlink_msg_gps_status_send(chan,g_gps->num_sats,NULL,NULL,NULL,NULL,NULL);
break;
}

2
ArduCopterMega/Parameters.h
View File

@ -223,7 +223,7 @@ public:
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
milliamp_hours (CURR_AMP_HOURS, k_param_milliamp_hours, PSTR("MAH")),
compass_enabled (DISABLED, k_param_compass_enabled, PSTR("COMPASS_ENABLE")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),

93
ArduCopterMega/command description.txt
View File

@ -1,80 +1,61 @@
ArduPilotMega 1.0.0 Commands
ArduPilotMega 2.0 Commands
Command Structure in bytes
0 0x00 byte Command ID
1 0x01 byte Parameter 1
2 0x02 int Parameter 2
2 0x02 long Parameter 2
3 0x03 ..
4 0x04 long Parameter 3
4 0x04 ..
5 0x05 ..
6 0x06 ..
6 0x06 long Parameter 3
7 0x07 ..
8 0x08 long Parameter 4
8 0x08 ..
9 0x09 ..
10 0x0A ..
10 0x0A long Parameter 4
11 0x0B ..
11 0x0C ..
11 0x0D ..
0x00 Reserved
....
0x0F Reserved
***********************************
Commands 0x10 to 0x2F are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
Commands below MAV_CMD_NAV_LAST 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
0x19 CMD_TARGET - altitude lat lon
Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) - altitude lat lon
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
***********************************
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)
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (milliseconds) -
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - -
0x72 / 114 MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL)
0x23 MAV_CMD_CONDITION_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
Unexecuted commands > MAV_CMD_NAV_LAST are dropped when ready for the next command < MAV_CMD_NAV_LAST so plan/queue commands accordingly!
For example if you had a string of CMD_MAV_CONDITION commands following a 0x10 command that had not finished when the waypoint was
reached, the unexecuted CMD_MAV_CONDITION and CMD_MAV_DO commands would be skipped and the next command < MAV_CMD_NAV_LAST 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
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0xB1 / 177 MAV_CMD_DO_JUMP index repeat count
0XB2 / 178 MAV_CMD_DO_CHANGE_SPEED TODO - Fill in from protocol
0xB3 / 179 MAV_CMD_DO_SET_HOME
0xB4 / 180 MAV_CMD_DO_SET_PARAMETER
0xB5 / 181 MAV_CMD_DO_SET_RELAY
0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY
0xB7 / 183 MAV_CMD_DO_SET_SERVO
0xB8 / 184 MAV_CMD_DO_REPEAT_SERVO

43
ArduCopterMega/commands.pde
View File

@ -111,13 +111,35 @@ set_current_loc_here()
set_next_WP(&l);
}
void loiter_at_location()
{
next_WP = current_loc;
}
void set_mode_loiter_home(void)
{
control_mode = LOITER;
//crash_timer = 0;
next_WP = current_loc;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
// output control mode to the ground station
gcs.send_message(MSG_HEARTBEAT);
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
//********************************************************************************
//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)
void return_to_launch(void)
{
//so we know where we are navigating from
next_WP = current_loc;
@ -137,8 +159,7 @@ return_to_launch(void)
//send_message(SEVERITY_LOW,"Return To Launch");
}
struct
Location get_LOITER_home_wp()
struct Location get_LOITER_home_wp()
{
// read home position
struct Location temp = get_wp_with_index(0);
@ -151,8 +172,7 @@ Location get_LOITER_home_wp()
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)
void set_next_WP(struct Location *wp)
{
//GCS.send_text(SEVERITY_LOW,"load WP");
SendDebug("MSG <set_next_wp> wp_index: ");
@ -173,8 +193,12 @@ set_next_WP(struct Location *wp)
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt; // PH: target_altitude = 200
offset_altitude = next_WP.alt - prev_WP.alt; // PH: offset_altitude = 0
target_altitude = current_loc.alt;
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
offset_altitude = next_WP.alt - prev_WP.alt;
else
offset_altitude = 0;
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
@ -223,7 +247,8 @@ void init_home()
// ground altitude in centimeters for pressure alt calculations
// ------------------------------------------------------------
save_EEPROM_pressure();
g.ground_pressure.save();
// Save Home to EEPROM
// -------------------

285
ArduCopterMega/commands_logic.pde Normal file
View File

@ -0,0 +1,285 @@
void
handle_no_commands()
{
switch (control_mode){
case LAND:
// don't get a new command
break;
default:
next_command = get_LOITER_home_wp();
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
//SendDebugln(next_command.id,DEC);
break;
}
}
void
handle_process_must(byte id)
{
// reset navigation integrators
// -------------------------
reset_I();
switch(id){
case MAV_CMD_NAV_TAKEOFF: // TAKEOFF!
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
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
next_WP.alt = current_loc.alt + takeoff_altitude;
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
//set_next_WP(&next_WP);
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
break;
//case MAV_CMD_NAV_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 MAV_CMD_NAV_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 g_gps ground course during TO. IMU will be doing yaw drift correction
break;
/*
case MAV_CMD_ALTITUDE: //
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 MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
break;
case MAV_CMD_NAV_LOITER_TIME:
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return_to_launch();
break;
}
}
void
handle_process_may(byte id)
{
switch(id){
case MAV_CMD_CONDITION_DELAY: // Navigate to Waypoint
delay_start = millis();
delay_timeout = next_command.lat;
break;
//case MAV_CMD_NAV_LAND_OPTIONS: // Land this puppy
// break;
case MAV_CMD_CONDITION_YAW:
// p1: bearing
// alt: speed
// lat: direction (-1,1),
// lng: rel (1) abs (0)
// target angle in degrees
command_yaw_start = nav_yaw; // current position
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);
}else{
// absolute
command_yaw_end = next_command.p1 * 100;
}
// 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;
//9000 turn in 10 seconds
//command_yaw_time = 9000/ 10 = 900° per second
break;
default:
break;
}}
void
handle_process_now(byte id)
{
switch(id){
case MAV_CMD_DO_SET_HOME:
init_home();
break;
case MAV_CMD_DO_REPEAT_SERVO:
new_event(&next_command);
break;
case MAV_CMD_DO_SET_SERVO:
//Serial.print("MAV_CMD_DO_SET_SERVO ");
//Serial.print(next_command.p1,DEC);
//Serial.print(" PWM: ");
//Serial.println(next_command.alt,DEC);
APM_RC.OutputCh(next_command.p1, next_command.alt);
break;
case MAV_CMD_DO_SET_RELAY:
if (next_command.p1 == 0) {
relay_on();
} else if (next_command.p1 == 1) {
relay_off();
}else{
relay_toggle();
}
break;
}
}
// Verify commands
// ---------------
void verify_must()
{
switch(command_must_ID) {
/*case MAV_CMD_ALTITUDE:
if (abs(next_WP.alt - current_loc.alt) < 100){
command_must_index = 0;
}
break;
*/
case MAV_CMD_NAV_TAKEOFF: // Takeoff!
if (current_loc.alt > (next_WP.alt -100)){
command_must_index = 0;
takeoff_complete = true;
}
break;
case MAV_CMD_NAV_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;
}
update_crosstrack();
break;
case MAV_CMD_NAV_WAYPOINT: // reach a waypoint
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
//SendDebugln(command_must_index,DEC);
char message[50];
sprintf(message,"Reached Waypoint #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
// 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 MAV_CMD_NAV_LOITER_TURNS: // LOITER N times
break;
case MAV_CMD_NAV_LOITER_TIME: // loiter N milliseconds
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (wp_distance <= g.waypoint_radius) {
gcs.send_text(SEVERITY_LOW,"Reached home");
command_must_index = 0;
}
break;
//case MAV_CMD_NAV_LOITER_UNLIM: // Just plain LOITER
// break;
default:
gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
break;
}
}
void verify_may()
{
float power;
switch(command_may_ID) {
case MAV_CMD_CONDITION_ANGLE:
if((millis() - command_yaw_start_time) > command_yaw_time){
command_must_index = 0;
nav_yaw = command_yaw_end;
}else{
// else we need to be at a certain place
// power is a ratio of the time : .5 = half done
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;
case MAV_CMD_CONDITION_DELAY:
if ((millis() - delay_start) > delay_timeout){
command_may_index = 0;
delay_timeout = 0;
}
//case CMD_LAND_OPTIONS:
// break;
}
}

326
ArduCopterMega/commands_process.pde
View File

@ -42,18 +42,7 @@ void load_next_command()
//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
//SendDebugln(g.waypoint_index,DEC);
switch (control_mode){
case LAND:
// don't get a new command
break;
default:
next_command = get_LOITER_home_wp();
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
//SendDebugln(next_command.id,DEC);
break;
}
handle_no_commands();
}
}
@ -125,62 +114,7 @@ void process_must()
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
// reset navigation integrators
// -------------------------
reset_I();
switch(command_must_ID){
case MAV_CMD_NAV_TAKEOFF: // TAKEOFF!
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
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
next_WP.alt = current_loc.alt + takeoff_altitude;
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
//set_next_WP(&next_WP);
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
break;
//case MAV_CMD_NAV_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 MAV_CMD_NAV_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 g_gps ground course during TO. IMU will be doing yaw drift correction
break;
/*
case MAV_CMD_ALTITUDE: //
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 MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
break;
case MAV_CMD_NAV_LOITER_TIME:
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return_to_launch();
break;
}
handle_process_must(command_must_ID);
}
void process_may()
@ -195,79 +129,8 @@ void process_may()
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// do the command
// --------------
switch(command_may_ID){
case MAV_CMD_CONDITION_DELAY: // Navigate to Waypoint
delay_start = millis();
delay_timeout = next_command.lat;
break;
//case MAV_CMD_NAV_LAND_OPTIONS: // Land this puppy
// break;
case MAV_CMD_CONDITION_YAW:
// p1: bearing
// alt: speed
// lat: direction (-1,1),
// lng: rel (1) abs (0)
// target angle in degrees
command_yaw_start = nav_yaw; // current position
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);
} else {
// absolute
command_yaw_end = next_command.p1 * 100;
}
// 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;
//9000 turn in 10 seconds
//command_yaw_time = 9000/ 10 = 900° per second
break;
default:
break;
}
handle_process_may(command_may_ID);
}
void process_now()
@ -284,187 +147,6 @@ void process_now()
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// do the command
// --------------
switch(id){
/*
case CMD_THROTTLE_CRUISE:
g.throttle_cruise = next_command.p1;
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 MAV_CMD_DO_SET_HOME:
init_home();
break;
case MAV_CMD_DO_REPEAT_SERVO:
new_event(&next_command);
break;
case MAV_CMD_DO_SET_SERVO:
//Serial.print("MAV_CMD_DO_SET_SERVO ");
//Serial.print(next_command.p1,DEC);
//Serial.print(" PWM: ");
//Serial.println(next_command.alt,DEC);
APM_RC.OutputCh(next_command.p1, next_command.alt);
break;
case MAV_CMD_DO_SET_RELAY:
if (next_command.p1 == 0) {
relay_on();
} else if (next_command.p1 == 1) {
relay_off();
}else{
relay_toggle();
}
break;
}
}
// Verify commands
// ---------------
void verify_must()
{
switch(command_must_ID) {
/*case MAV_CMD_ALTITUDE:
if (abs(next_WP.alt - current_loc.alt) < 100){
command_must_index = 0;
}
break;
*/
case MAV_CMD_NAV_TAKEOFF: // Takeoff!
if (current_loc.alt > (next_WP.alt -100)){
command_must_index = 0;
takeoff_complete = true;
}
break;
case MAV_CMD_NAV_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;
}
update_crosstrack();
break;
case MAV_CMD_NAV_WAYPOINT: // reach a waypoint
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
//SendDebugln(command_must_index,DEC);
char message[50];
sprintf(message,"Reached Waypoint #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
// 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 MAV_CMD_NAV_LOITER_TURNS: // LOITER N times
break;
case MAV_CMD_NAV_LOITER_TIME: // loiter N milliseconds
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (wp_distance <= g.waypoint_radius) {
gcs.send_text(SEVERITY_LOW,"Reached home");
command_must_index = 0;
}
break;
//case MAV_CMD_NAV_LOITER_UNLIM: // Just plain LOITER
// break;
default:
gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
break;
}
}
void verify_may()
{
float power;
switch(command_may_ID) {
case MAV_CMD_CONDITION_ANGLE:
if((millis() - command_yaw_start_time) > command_yaw_time){
command_must_index = 0;
nav_yaw = command_yaw_end;
}else{
// else we need to be at a certain place
// power is a ratio of the time : .5 = half done
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;
case MAV_CMD_CONDITION_DELAY:
if ((millis() - delay_start) > delay_timeout){
command_may_index = 0;
delay_timeout = 0;
}
//case CMD_LAND_OPTIONS:
// break;
}
handle_process_now(id);
}

118
ArduCopterMega/config.h
View File

@ -26,7 +26,7 @@
/// 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
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
///
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
/// change in your local copy of APM_Config.h.
@ -42,24 +42,6 @@
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// ENABLE_HIL
// Hardware in the loop output
//
#ifndef ENABLE_HIL
# define ENABLE_HIL DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GCS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
//////////////////////////////////////////////////////////////////////////////
// FRAME_CONFIG
@ -75,32 +57,81 @@
# define SONAR_PIN AP_RANGEFINDER_PITOT_TUBE
#endif
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_SENSOR
// AIRSPEED_RATIO
//
#ifndef AIRSPEED_SENSOR
# define AIRSPEED_SENSOR DISABLED
#endif
#ifndef AIRSPEED_RATIO
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_PROTOCOL OPTIONAL
// HIL_MODE OPTIONAL
// HIL_PORT OPTIONAL
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#ifndef HIL_PROTOCOL
# error Must define HIL_PROTOCOL if HIL_MODE is not disabled
#endif
#ifndef HIL_PORT
# error Must define HIL_PORT if HIL_PROTOCOL is set
#endif
#endif
// If we are in XPlane, diasble the mag
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
// check xplane settings
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
// MAGNETOMETER not supported by XPLANE
# undef MAGNETOMETER
# define MAGNETOMETER DISABLED
# if HIL_MODE != HIL_MODE_ATTITUDE
# error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE
# endif
#endif
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL
// GCS_PORT
//
#ifndef GCS_PROTOCOL
# define GCS_PROTOCOL GCS_PROTOCOL_STANDARD
# define GCS_PROTOCOL GCS_PROTOCOL_NONE
#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
# define SERIAL0_BAUD 115200
#endif
#ifndef SERIAL3_BAUD
# define SERIAL3_BAUD 115200
# define SERIAL3_BAUD 57600
#endif
@ -119,6 +150,7 @@
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.0
#endif
#ifndef CURR_VOLT_DIV_RATIO
# define CURR_VOLT_DIV_RATIO 15.7
#endif
@ -137,28 +169,15 @@
# define INPUT_VOLTAGE 5.0
#endif
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGORIENTATION
# define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_FORWARD
#ifndef MAGNETOMETER
# define MAGNETOMETER DISABLED
#endif
// run the CLI tool to get this value
#ifndef MAGOFFSET
# define MAGOFFSET 0,0,0
#ifndef MAG_ORIENTATION
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#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
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
@ -366,7 +385,6 @@
#ifndef PITCH_MAX
# define PITCH_MAX 36
#endif
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
//////////////////////////////////////////////////////////////////////////////
@ -427,8 +445,6 @@
#ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 30 // deg
#endif
//# define XTRACK_GAIN_SCALED XTRACK_GAIN * 100
//# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE * 100
//////////////////////////////////////////////////////////////////////////////

9
ArduCopterMega/control_modes.pde
View File

@ -8,6 +8,7 @@ void read_control_switch()
set_mode(g.flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
prev_WP = current_loc;
// reset navigation integrators
// -------------------------
@ -40,8 +41,8 @@ void reset_control_switch()
{
oldSwitchPosition = -1;
read_control_switch();
//Serial.print("MSG: reset_control_switch");
//Serial.println(oldSwitchPosition , DEC);
SendDebug("MSG: reset_control_switch");
SendDebugln(oldSwitchPosition , DEC);
}
void update_servo_switches()
@ -74,7 +75,7 @@ void read_trim_switch()
if((millis() - trim_timer) > 2000){
imu.save();
} else {
}else{
// set the throttle nominal
if(g.rc_3.control_in > 50){
g.throttle_cruise.set(g.rc_3.control_in);
@ -87,7 +88,7 @@ void read_trim_switch()
// this is a test for Max's tri-copter
if(g.frame_type == TRI_FRAME){
g.rc_4.trim(); // yaw
g.rc_4.save_trim();
g.rc_4.save_eeprom();
}
}
trim_flag = false;

119
ArduCopterMega/defines.h
View File

@ -1,5 +1,26 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// ACM:
// Motors
#define RIGHT CH_1
#define LEFT CH_2
#define FRONT CH_3
#define BACK CH_4
#define RIGHTFRONT CH_7
#define LEFTBACK CH_8
#define MAX_SERVO_OUTPUT 2700
// active altitude sensor
#define SONAR 0
#define BARO 1
// Frame types
#define PLUS_FRAME 0
#define X_FRAME 1
#define TRI_FRAME 2
#define HEXA_FRAME 3
// Internal defines, don't edit and expect things to work
// -------------------------------------------------------
@ -14,16 +35,6 @@
#define T6 1000000
#define T7 10000000
// 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
@ -52,8 +63,6 @@
#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
// HIL enumerations
#define HIL_PROTOCOL_XPLANE 1
@ -63,37 +72,13 @@
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
// motors
#define RIGHT CH_1
#define LEFT CH_2
#define FRONT CH_3
#define BACK CH_4
#define RIGHTFRONT CH_7
#define LEFTBACK CH_8
#define MAX_SERVO_OUTPUT 2700
#define SONAR 0
#define BARO 1
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // 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
#define TRI_FRAME 2
#define HEXA_FRAME 3
#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol
#define GCS_PROTOCOL_LEGACY 1 // legacy ArduPilot protocol
#define GCS_PROTOCOL_XPLANE 2 // X-Plane HIL simulation
#define GCS_PROTOCOL_DEBUGTERMINAL 3 //Human-readable debug interface for use with a dumb terminal
#define GCS_PROTOCOL_MAVLINK 4 // binary protocol for qgroundcontrol
#define GCS_PROTOCOL_NONE -1 // No GCS output
// Auto Pilot modes
// ----------------
@ -102,15 +87,17 @@
#define ALT_HOLD 2 // AUTO control
#define FBW 3 // AUTO control
#define AUTO 4 // AUTO control
#define POSITION_HOLD 5 // Hold a single location
#define RTL 6 // AUTO control
#define TAKEOFF 7 // controlled decent rate
#define LAND 8 // controlled decent rate
#define NUM_MODES 9
#define LOITER 5 // AUTO control
#define POSITION_HOLD 6 // Hold a single location
#define RTL 7 // AUTO control
#define TAKEOFF 8 // controlled decent rate
#define LAND 9 // controlled decent rate
#define NUM_MODES 10
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0x00 // there is no command stored in the mem location requested
//repeating events
#define NO_REPEAT 0
#define CH_4_TOGGLE 1
@ -183,7 +170,6 @@
#define SEVERITY_HIGH 3
#define SEVERITY_CRITICAL 4
// Logging parameters
#define LOG_INDEX_MSG 0xF0
#define LOG_ATTITUDE_MSG 0x01
@ -211,6 +197,18 @@
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
// 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)
#define LOGBIT_CURRENT (1<<9)
// Waypoint Modes
// ----------------
#define ABS_WP 0
@ -229,6 +227,10 @@
#define EVENT_LOADED_WAYPOINT 3
#define EVENT_LOOP 4
// Climb rate calculations
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define CURRENT_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_VOLT_DIV_RATIO
@ -257,24 +259,9 @@
#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
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// 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)
#define LOGBIT_CURRENT (1<<9)
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_SIZE 14

5
ArduCopterMega/motors.pde
View File

@ -97,11 +97,6 @@ set_servos_4()
//Serial.println("TRI_FRAME");
// Tri-copter power distribution
float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .7, 1.0);
return temp;
}
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;

111
ArduCopterMega/navigation.pde
View File

@ -7,8 +7,7 @@ void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (g_gps->fix == 0)
{
if (g_gps->fix == 0){
g_gps->new_data = false;
return;
}
@ -19,10 +18,10 @@ void navigate()
// waypoint distance from plane
// ----------------------------
GPS_wp_distance = getDistance(&current_loc, &next_WP);
wp_distance = getDistance(&current_loc, &next_WP);
if (GPS_wp_distance < 0){
send_message(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
if (wp_distance < 0){
gcs.send_text(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return;
@ -32,10 +31,21 @@ void navigate()
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrack correction
// -------------------------------------------
// nav_bearing will includes xtrac correction
// ------------------------------------------
nav_bearing = target_bearing;
// check if we have missed the WP
loiter_delta = (target_bearing - old_target_bearing)/100;
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta);
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
@ -56,14 +66,6 @@ void calc_nav()
pitch_max = 22° (2200)
*/
//temp = dcm.get_dcm_matrix();
//yawvector.y = temp.b.x; // cos
//yawvector.x = temp.a.x; // sin
//yawvector.normalize();
//cos_yaw_x = yawvector.y; // 0
//sin_yaw_y = yawvector.x; // 1
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 50 - 30 = 20 pitch right
lat_error = next_WP.lat - current_loc.lat; // 50 - 30 = 20 pitch up
@ -88,46 +90,12 @@ void calc_nav()
nav_pitch = constrain(nav_pitch, -g.pitch_max.get(), g.pitch_max.get());
}
/*
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 - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_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)g_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()
{
if(control_mode == AUTO && offset_altitude != 0) {
@ -140,19 +108,13 @@ void calc_altitude_error()
}else{
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
}
} else {
}else{
target_altitude = next_WP.alt;
}
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);
}
// target_altitude = current_loc.alt; // PH: target_altitude = -200
// offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0
long wrap_360(long error)
{
if (error > 36000) error -= 36000;
@ -167,18 +129,33 @@ long wrap_180(long error)
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;
} */
float power;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
nav_bearing += (int)(9000.0 * (2.0 + power));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
if (wp_distance < g.loiter_radius){
nav_bearing += 9000;
}else{
nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
}
update_crosstrack;
nav_bearing = wrap_360(nav_bearing);
}
void update_crosstrack(void)
{
@ -196,7 +173,7 @@ void reset_crosstrack()
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
int get_altitude_above_home(void)
long get_altitude_above_home(void)
{
// This is the altitude above the home location
// The GPS gives us altitude at Sea Level

68
ArduCopterMega/radio.pde
View File

@ -2,26 +2,19 @@ void init_rc_in()
{
read_EEPROM_radio(); // read Radio limits
g.rc_1.set_angle(4500);
g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.set_angle(4500);
g.rc_2.dead_zone = 60;
g.rc_3.set_range(0,1000);
g.rc_3.dead_zone = 20;
g.rc_3.scale_output = .9;
g.rc_4.set_angle(6000);
g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.dead_zone = 60;
g.rc_3.dead_zone = 20;
g.rc_4.dead_zone = 500;
g.rc_5.set_range(0,1000);
g.rc_5.set_filter(false);
// for kP values
//g.rc_6.set_range(200,800);
//g.rc_6.set_range(0,1800); // for faking GPS
g.rc_6.set_range(0,1000);
// for camera angles
//g.rc_6.set_angle(4500);
//g.rc_6.dead_zone = 60;
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
}
@ -66,6 +59,50 @@ void read_radio()
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), g.rc_1.control_in, g.rc_2.control_in, g.rc_3.control_in, g.rc_4.control_in);
}
/*
void throttle_failsafe(uint16_t pwm)
{
if(g.throttle_fs_enabled == 0)
return;
//check for failsafe and debounce funky reads
// ------------------------------------------
if (pwm < g.throttle_fs_value){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9){
SendDebug("MSG FS ON ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 10) {
ch3_failsafe = true;
//set_failsafe(true);
//failsafeCounter = 10;
}else if (failsafeCounter > 10){
failsafeCounter = 11;
}
}else if(failsafeCounter > 0){
// we are no longer in failsafe condition
// but we need to recover quickly
failsafeCounter--;
if (failsafeCounter > 3){
failsafeCounter = 3;
}
if (failsafeCounter == 1){
SendDebug("MSG FS OFF ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 0) {
ch3_failsafe = false;
//set_failsafe(false);
//failsafeCounter = -1;
}else if (failsafeCounter <0){
failsafeCounter = -1;
}
}
}
*/
void trim_radio()
{
for (byte i = 0; i < 30; i++){
@ -76,9 +113,9 @@ void trim_radio()
g.rc_2.trim(); // pitch
g.rc_4.trim(); // yaw
g.rc_1.save_trim();
g.rc_2.save_trim();
g.rc_4.save_trim();
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
g.rc_4.save_eeprom();
}
void trim_yaw()
@ -88,3 +125,4 @@ void trim_yaw()
}
g.rc_4.trim(); // yaw
}

16
ArduCopterMega/sensors.pde
View File

@ -1,7 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
void ReadSCP1000(void) {}
void init_pressure_ground(void)
void init_barometer(void)
{
for(int i = 0; i < 300; i++){ // We take some readings...
delay(20);
@ -12,8 +13,10 @@ void init_pressure_ground(void)
abs_pressure = barometer.Press;
}
long
read_barometer(void)
// Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
long read_barometer(void)
{
float x, scaling, temp;
@ -24,6 +27,7 @@ read_barometer(void)
scaling = (float)ground_pressure / (float)abs_pressure;
temp = ((float)ground_temperature / 10.0f) + 273.15f;
x = log(scaling) * temp * 29271.267f;
return (x / 10);
}
@ -33,6 +37,10 @@ void read_airspeed(void)
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
#if BATTERY_EVENT == 1
void read_battery(void)
{

54
ArduCopterMega/setup.pde
View File

@ -102,26 +102,6 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
// note, cannot actually return here
return(0);
//zero_eeprom();
//default_gains();
// setup default values
/*
default_waypoint_info();
default_nav();
default_alt_hold();
default_frame();
default_flight_modes();
default_throttle();
default_logs();
default_current();
print_done();
*/
// finish
// ------
//return(0);
}
// Perform radio setup.
@ -191,14 +171,9 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_8.update_min_max();
if(Serial.available() > 0){
//g.rc_3.radio_max += 250;
Serial.flush();
save_EEPROM_radio();
//delay(100);
// double checking
//read_EEPROM_radio();
//print_radio_values();
print_done();
break;
}
@ -445,7 +420,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.compass_enabled = false;
} else {
}else{
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
report_compass();
return 0;
@ -471,7 +446,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
g.frame_type = HEXA_FRAME;
} else {
}else{
Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n"));
report_frame();
return 0;
@ -486,23 +461,20 @@ static int8_t
setup_current(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.current_enabled.set(true);
save_EEPROM_mag();
g.current_enabled.set_and_save(true);
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.current_enabled.set(false);
save_EEPROM_mag();
g.current_enabled.set_and_save(false);
} else if(argv[1].i > 10){
g.milliamp_hours = argv[1].i;
g.milliamp_hours.set_and_save(argv[1].i);
} else {
}else{
Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
report_current();
return 0;
}
save_EEPROM_current();
report_current();
return 0;
}
@ -516,7 +488,7 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.sonar_enabled.set_and_save(false);
} else {
}else{
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
report_sonar();
return 0;
@ -604,7 +576,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
/***************************************************************************/
// CLI utilities
// CLI defaults
/***************************************************************************/
void default_waypoint_info()
@ -769,7 +741,7 @@ default_gains()
/***************************************************************************/
// CLI utilities
// CLI reports
/***************************************************************************/
void report_current()
@ -938,7 +910,6 @@ void report_compass()
print_blanks(2);
}
void report_flight_modes()
{
//print_blanks(2);
@ -1006,9 +977,7 @@ print_divider(void)
Serial.println("");
}
// for reading in vales for mode switch
boolean
int8_t
radio_input_switch(void)
{
static int8_t bouncer = 0;
@ -1028,7 +997,7 @@ radio_input_switch(void)
if (bouncer == 1 || bouncer == -1) {
return bouncer;
} else {
}else{
return 0;
}
}
@ -1044,7 +1013,6 @@ void zero_eeprom(void)
Serial.printf_P(PSTR("done\n"));
}
void print_enabled(boolean b)
{
if(b)

6
ArduCopterMega/system.pde
View File

@ -103,7 +103,7 @@ void init_ardupilot()
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
}else{
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
@ -228,7 +228,7 @@ void startup_ground(void)
// read Baro pressure at ground
//-----------------------------
init_pressure_ground();
init_barometer();
// initialize commands
// -------------------
@ -344,7 +344,7 @@ void update_GPS_light(void)
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (GPS_light){
digitalWrite(C_LED_PIN, LOW);
} else {
}else{
digitalWrite(C_LED_PIN, HIGH);
}
g_gps->valid_read = false;

160
ArduCopterMega/test.pde
View File

@ -1,3 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// 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);
@ -8,8 +10,8 @@ static int8_t test_fbw(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
static int8_t test_dcm(uint8_t argc, const Menu::arg *argv);
static int8_t test_omega(uint8_t argc, const Menu::arg *argv);
//static int8_t test_dcm(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_current(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
@ -47,8 +49,8 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"gps", test_gps},
{"adc", test_adc},
{"imu", test_imu},
{"dcm", test_dcm},
{"omega", test_omega},
//{"dcm", test_dcm},
//{"omega", test_omega},
{"battery", test_battery},
{"current", test_current},
{"relay", test_relay},
@ -97,7 +99,15 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
// ----------------------------------------------------------
read_radio();
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.rc_1.radio_in, g.rc_2.radio_in, g.rc_3.radio_in, g.rc_4.radio_in, g.rc_5.radio_in, g.rc_6.radio_in, g.rc_7.radio_in, g.rc_8.radio_in);
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.rc_1.radio_in,
g.rc_2.radio_in,
g.rc_3.radio_in,
g.rc_4.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in);
if(Serial.available() > 0){
return (0);
@ -125,7 +135,15 @@ test_radio(uint8_t argc, const Menu::arg *argv)
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (g.rc_1.control_in), (g.rc_2.control_in), (g.rc_3.control_in), (g.rc_4.control_in), g.rc_5.control_in, g.rc_6.control_in, g.rc_7.control_in);
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
(g.rc_1.control_in),
(g.rc_2.control_in),
(g.rc_3.control_in),
(g.rc_4.control_in),
g.rc_5.control_in,
g.rc_6.control_in,
g.rc_7.control_in);
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
/*Serial.printf_P(PSTR( "min: %d"
@ -439,8 +457,6 @@ test_imu(uint8_t argc, const Menu::arg *argv)
Vector3f accels = imu.get_accel();
Vector3f gyros = imu.get_gyro();
update_trig();
if(g.compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
@ -452,16 +468,18 @@ test_imu(uint8_t argc, const Menu::arg *argv)
// We are using the IMU
// ---------------------
/*
Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
"G: %4.4f, %4.4f, %4.4f\t"),
accels.x, accels.y, accels.z,
gyros.x, gyros.y, gyros.z);
*/
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\t"),
dcm.roll_sensor,
dcm.pitch_sensor,
dcm.yaw_sensor);
/*
update_trig();
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
cos_pitch_x,
sin_pitch_y,
@ -469,6 +487,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
sin_roll_y,
cos_yaw_x, // x
sin_yaw_y); // y
*/
}
if(Serial.available() > 0){
@ -483,43 +502,31 @@ test_gps(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
delay(1000);
/*while(1){
delay(100);
update_GPS();
if(Serial.available() > 0){
return (0);
}
if(home.lng != 0){
break;
}
}*/
while(1){
delay(100);
update_GPS();
delay(333);
calc_distance_error();
// Blink GPS LED if we don't have a fix
// ------------------------------------
update_GPS_light();
//if (g_gps->new_data){
Serial.printf_P(PSTR("Lat: %3.8f, Lon: %3.8f, alt %dm, spd: %d dist:%d, #sats: %d\n"),
((float)g_gps->latitude / 10000000),
((float)g_gps->longitude / 10000000),
(int)g_gps->altitude / 100,
(int)g_gps->ground_speed,
(int)wp_distance,
(int)g_gps->num_sats);
//}else{
//Serial.print(".");
//}
g_gps->update();
if (g_gps->new_data){
Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
g_gps->latitude,
g_gps->longitude,
g_gps->altitude/100,
g_gps->num_sats);
}else{
Serial.print(".");
}
if(Serial.available() > 0){
return (0);
}
}
}
/*
static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv)
{
@ -569,8 +576,7 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
}
}
}
*/
/*
static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv)
@ -594,7 +600,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
}
}
*/
static int8_t
/*static int8_t
test_omega(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
@ -629,6 +636,7 @@ test_omega(uint8_t argc, const Menu::arg *argv)
}
return (0);
}
*/
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
@ -638,14 +646,11 @@ test_battery(uint8_t argc, const Menu::arg *argv)
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);
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n")
battery_voltage1,
battery_voltage2,
battery_voltage3,
battery_voltage4);
#else
Serial.printf_P(PSTR("Not enabled\n"));
@ -663,7 +668,10 @@ test_current(uint8_t argc, const Menu::arg *argv)
delay(100);
read_radio();
read_current();
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), current_voltage, current_amps, current_total);
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
current_voltage,
current_amps,
current_total);
//if(g.rc_3.control_in > 0){
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
@ -678,10 +686,6 @@ test_current(uint8_t argc, const Menu::arg *argv)
}
}
static int8_t
test_relay(uint8_t argc, const Menu::arg *argv)
{
@ -689,14 +693,14 @@ test_relay(uint8_t argc, const Menu::arg *argv)
delay(1000);
while(1){
Serial.println("Relay on");
Serial.printf_P(PSTR("Relay on\n"));
relay_on();
delay(3000);
if(Serial.available() > 0){
return (0);
}
Serial.println("Relay off");
Serial.printf_P(PSTR("Relay off\n"));
relay_off();
delay(3000);
if(Serial.available() > 0){
@ -725,12 +729,24 @@ test_wp(uint8_t argc, const Menu::arg *argv)
for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_wp_with_index(i);
print_waypoint(&temp, i);
test_wp_print(&temp, i);
}
return (0);
}
void
test_wp_print(struct Location *cmd, byte index)
{
Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)index,
(int)cmd->id,
(int)cmd->p1,
cmd->alt,
cmd->lat,
cmd->lng);
}
static int8_t
test_xbee(uint8_t argc, const Menu::arg *argv)
@ -772,7 +788,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
home.alt = 0;
wp_distance = 0;
init_pressure_ground();
init_barometer();
while(1){
if (millis()-fast_loopTimer > 9) {
@ -828,32 +844,30 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
static int8_t
test_mag(uint8_t argc, const Menu::arg *argv)
{
if(g.compass_enabled == false){
Serial.printf_P(PSTR("Compass disabled\n"));
return (0);
}else{
if(g.compass_enabled) {
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){
Serial.printf_P(PSTR("Heading: %4.2f, XYZ: %4.2f, %4.2f, %4.2f"),
ToDeg(compass.heading),
compass.mag_x,
compass.mag_y,
compass.mag_z);
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
}
void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));