diff --git a/ArduCopterMega/APM_Config.h.reference b/ArduCopterMega/APM_Config.h.reference index 46dfb00ab6..68b433b5a2 100644 --- a/ArduCopterMega/APM_Config.h.reference +++ b/ArduCopterMega/APM_Config.h.reference @@ -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. diff --git a/ArduCopterMega/APM_Config_mavlink_hil.h b/ArduCopterMega/APM_Config_mavlink_hil.h new file mode 100644 index 0000000000..1f1687b0e6 --- /dev/null +++ b/ArduCopterMega/APM_Config_mavlink_hil.h @@ -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 diff --git a/ArduCopterMega/APM_Config_xplane.h b/ArduCopterMega/APM_Config_xplane.h new file mode 100644 index 0000000000..dddc24af1c --- /dev/null +++ b/ArduCopterMega/APM_Config_xplane.h @@ -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 diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index dcfeba6458..0c62688d9b 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -26,17 +26,18 @@ version 2.1 of the License, or (at your option) any later version. // Libraries #include #include -#include // ArduPilot Mega RC Library -#include // ArduPilot GPS library +#include +#include // ArduPilot Mega RC Library +#include // ArduPilot GPS library #include // Arduino I2C lib -#include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include // ArduPilot Mega BMP085 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 +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega BMP085 Library +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega IMU Library +#include // ArduPilot Mega DCM Library +#include // PID library #include // RC Channel Library #include // 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; diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index ec6ad95fbe..31996b55e3 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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(); diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 5720e9d142..79e510a0ec 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -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 diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 1b6c93552f..4f96c59a3e 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -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 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... } } diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 2643edd682..5c7ebbadd6 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -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; } diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index e297a86073..4987ce2f1f 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -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")), diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt index 4fd8018e51..d851988808 100644 --- a/ArduCopterMega/command description.txt +++ b/ArduCopterMega/command description.txt @@ -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 diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index bf29dadf2e..cc1654f5f8 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -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 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 // ------------------- diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde new file mode 100644 index 0000000000..dff3455c4a --- /dev/null +++ b/ArduCopterMega/commands_logic.pde @@ -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 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 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," 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; + } +} + diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 90134a504e..1464c1a280 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -42,18 +42,7 @@ void load_next_command() //SendDebug("MSG 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 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," 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, " 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 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," 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); } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 8689dd3062..26010b5250 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -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 ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 75075d4ed7..6fc8da9614 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -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; diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 9eae6e6e9b..c2738eb8c7 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -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 diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index e51edc0e6a..c849c7ecb5 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -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; diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 421d69c6f5..7ec6866723 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -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(¤t_loc, &next_WP); + wp_distance = getDistance(¤t_loc, &next_WP); - if (GPS_wp_distance < 0){ - send_message(SEVERITY_HIGH," WP error - distance < 0"); + if (wp_distance < 0){ + gcs.send_text(SEVERITY_HIGH," WP error - distance < 0"); //Serial.println(wp_distance,DEC); //print_current_waypoints(); return; @@ -32,10 +31,21 @@ void navigate() // -------------------------------------------- target_bearing = get_bearing(¤t_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(¤t_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 diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index fa6d2f5d9f..e8b49fb1ad 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -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 } + diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index c9190f7afa..7dbe85a38a 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -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) { diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index ec9356596d..719611c2d4 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -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) diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index d732eb6d3c..32c9d512d5 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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; diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 03a4a73f9b..5ccad4edcb 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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"));