From 609ae8359de6d76003baca82393dc7954e9ca1b0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:45:13 +1000 Subject: [PATCH 01/74] import APM_Camera branch from SVN --- ArduPlane/APM_Config.h | 38 +- ArduPlane/APM_Config.h.reference | 36 +- ArduPlane/ArduPilotMega.pde | 361 +++++++------ ArduPlane/Attitude.pde | 139 ++--- ArduPlane/Cam_move.pde | 210 ++++++++ ArduPlane/Cam_trigger.pde | 41 ++ ArduPlane/GCS.h | 8 +- ArduPlane/GCS_Mavlink.pde | 484 ++++++----------- ArduPlane/HIL.h | 8 +- ArduPlane/HIL_Xplane.pde | 2 +- ArduPlane/Log.pde | 107 ++-- ArduPlane/Mavlink_Common.h | 158 +----- ArduPlane/Parameters.h | 844 ++++++++++++++---------------- ArduPlane/WP_activity.pde | 49 ++ ArduPlane/climb_rate.pde | 44 +- ArduPlane/command description.txt | 2 +- ArduPlane/commands.pde | 103 ++-- ArduPlane/commands_logic.pde | 108 ++-- ArduPlane/commands_process.pde | 20 +- ArduPlane/config.h | 117 +---- ArduPlane/control_modes.pde | 73 ++- ArduPlane/defines.h | 61 +-- ArduPlane/events.pde | 26 +- ArduPlane/navigation.pde | 57 +- ArduPlane/planner.pde | 9 +- ArduPlane/radio.pde | 66 +-- ArduPlane/sensors.pde | 40 +- ArduPlane/setup.pde | 145 ++--- ArduPlane/system.pde | 170 +++--- ArduPlane/test.pde | 135 +++-- 30 files changed, 1707 insertions(+), 1954 deletions(-) create mode 100644 ArduPlane/Cam_move.pde create mode 100644 ArduPlane/Cam_trigger.pde create mode 100644 ArduPlane/WP_activity.pde diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 3a8bbb504f..4006d33839 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,5 +1,4 @@ - - // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // their default values, place the appropriate #define statements here. @@ -22,4 +21,37 @@ #define HIL_PORT 0 #define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #define GCS_PORT 3 -*/ +*/ + +#define MAGNETOMETER ENABLED + +// ----- Camera definitions ------ +// ------------------------------ +#define CAMERA ENABLED +#define CAM_DEBUG DISABLED +// Comment out servos that you do not have +//#define CAM_SERVO 8 // Camera servo channel +#define CAM_ANGLE 30 // Set angle in degrees +//#define CAM_CLICK 45 // This is the position of the servo arm when it actually clicks +//#define OPEN_SERVO 5 // Retraction servo channel - my camera retracts yours might not. + +// Camera yaw (left-right) +#define YAW_SERVO 7 // Pan servo channel (can be pitch in stabilization) +#define YAW_REV 1 // output is normal = 1 output is reversed = -1 +#define YAW_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg +#define YAW_RANGE 90 // Pan Servo sweep in degrees +#define YAW_RATIO 10.31 // match this to the swing of your pan servo + +// Camera pitch (up-down) +#define PITCH_SERVO 6 // Tilt servo channel (can be roll in stabilization) +#define PITCH_REV 1 // output is normal = 1 output is reversed = -1 +#define PITCH_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg +#define PITCH_RANGE 90 // Tilt Servo sweep in degrees +#define PITCH_RATIO 10.31 // match this to the swing of your tilt servo + +// Camera roll (up-down) +#define ROLL_SERVO 6 // Tilt servo channel (can be roll in stabilization) +#define ROLL_REV 1 // output is normal = 1 output is reversed = -1 +#define ROLL_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg +#define ROLL_RANGE 90 // Tilt Servo sweep in degrees +#define ROLL_RATIO 10.31 // match this to the swing of your tilt servo diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 6d8685b3e3..18fa27d019 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -210,7 +210,7 @@ // INPUT_VOLTAGE OPTIONAL // // In order to have accurate pressure and battery voltage readings, this -// value should be set to the voltage measured at the processor. +// value should be set to the voltage measured on the 5V rail on the oilpan. // // See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail. // @@ -304,32 +304,6 @@ //#define FLIGHT_MODE_6 MANUAL // -////////////////////////////////////////////////////////////////////////////// -// RC_5_FUNCT OPTIONAL -// RC_6_FUNCT OPTIONAL -// RC_7_FUNCT OPTIONAL -// RC_8_FUNCT OPTIONAL -// -// The channel 5 through 8 function assignments allow configuration of those -// channels for use with differential ailerons, flaps, flaperons, or camera -// or intrument mounts -// -//#define RC_5_FUNCT RC_5_FUNCT_NONE -//etc. - -////////////////////////////////////////////////////////////////////////////// -// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed -// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed -// then the flap position shall be flap_1_percent. If the speed setpoint is below flap_2_speed -// then the flap position shall be flap_2_percent. Speed setpoint is the current value of -// airspeed_cruise (if airspeed sensor enabled) or throttle_cruise (if no airspeed sensor) - -// FLAP_1_PERCENT OPTIONAL -// FLAP_1_SPEED OPTIONAL -// FLAP_2_PERCENT OPTIONAL -// FLAP_2_SPEED OPTIONAL - - ////////////////////////////////////////////////////////////////////////////// // THROTTLE_FAILSAFE OPTIONAL // THROTTLE_FS_VALUE OPTIONAL @@ -387,7 +361,7 @@ // the aircraft will head for home in RTL mode. If the failsafe condition is // resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home -// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER +// If XX_FAILSAFE_ACTION is 2 and the applicable failsafe occurs while in AUTO or LOITER // mode the aircraft will continue in that mode ignoring the failsafe condition. // Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always @@ -398,8 +372,8 @@ // The default behaviour is to ignore failsafes in AUTO and LOITER modes. // //#define GCS_HEARTBEAT_FAILSAFE DISABLED -//#define SHORT_FAILSAFE_ACTION 0 -//#define LONG_FAILSAFE_ACTION 0 +//#define SHORT_FAILSAFE_ACTION 2 +//#define LONG_FAILSAFE_ACTION 2 ////////////////////////////////////////////////////////////////////////////// @@ -800,7 +774,7 @@ // // Limits the slew rate of the throttle, in percent per second. Helps // avoid sudden throttle changes, which can destabilise the aircraft. -// A setting of zero disables the feature. Range 1 to 100. +// A setting of zero disables the feature. // Default is zero (disabled). // // P_TO_T OPTIONAL diff --git a/ArduPlane/ArduPilotMega.pde b/ArduPlane/ArduPilotMega.pde index 6f2d3dd601..8a43a61634 100644 --- a/ArduPlane/ArduPilotMega.pde +++ b/ArduPlane/ArduPilotMega.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.23" +#define THISFIRMWARE "ArduPilotMega 2.2.0" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -29,7 +29,6 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega RC Library #include // ArduPilot GPS library #include // Arduino I2C lib -#include // Arduino SPI lib #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library #include // ArduPilot Mega BMP085 Library @@ -39,10 +38,10 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library -#include // Range finder library -#include +//#include // Range finder library + +#define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions -#include // Configuration #include "config.h" @@ -71,12 +70,12 @@ FastSerialPort3(Serial3); // Telemetry port // // Global parameters are all contained within the 'g' class. // -static Parameters g; +Parameters g; //////////////////////////////////////////////////////////////////////////////// // prototypes -static void update_events(void); +void update_events(void); //////////////////////////////////////////////////////////////////////////////// @@ -93,17 +92,23 @@ static void update_events(void); // // All GPS access should be through this pointer. -static GPS *g_gps; - -// flight modes convenience array -static AP_Int8 *flight_modes = &g.flight_mode1; +GPS *g_gps; #if HIL_MODE == HIL_MODE_DISABLED // real sensors -static AP_ADC_ADS7844 adc; -static APM_BMP085_Class barometer; -static AP_Compass_HMC5843 compass(Parameters::k_param_compass); +AP_ADC_ADS7844 adc; +APM_BMP085_Class barometer; +// MAG PROTOCOL +#if MAG_PROTOCOL == MAG_PROTOCOL_5843 +AP_Compass_HMC5843 compass(Parameters::k_param_compass); + +#elif MAG_PROTOCOL == MAG_PROTOCOL_5883L +AP_Compass_HMC5883L compass(Parameters::k_param_compass); + +#else + #error Unrecognised MAG_PROTOCOL setting. +#endif // MAG PROTOCOL // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -139,7 +144,6 @@ AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); #elif HIL_MODE == HIL_MODE_ATTITUDE -AP_ADC_HIL adc; AP_DCM_HIL dcm; AP_GPS_HIL g_gps_driver(NULL); AP_Compass_HIL compass; // never used @@ -185,31 +189,18 @@ AP_IMU_Shim imu; // never used GCS_Class gcs; #endif -//////////////////////////////////////////////////////////////////////////////// -// SONAR selection -//////////////////////////////////////////////////////////////////////////////// -// -ModeFilter sonar_mode_filter; - -#if SONAR_TYPE == MAX_SONAR_XL - AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); -#elif SONAR_TYPE == MAX_SONAR_LV - // XXX honestly I think these output the same values - // If someone knows, can they confirm it? - AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); -#endif +//AP_RangeFinder_MaxsonarXL sonar; //////////////////////////////////////////////////////////////////////////////// // Global variables //////////////////////////////////////////////////////////////////////////////// -byte control_mode = INITIALISING; +byte control_mode = MANUAL; byte oldSwitchPosition; // for remembering the control mode switch -bool inverted_flight = false; -static const char *comma = ","; +const char *comma = ","; -static const char* flight_mode_strings[] = { +const char* flight_mode_strings[] = { "Manual", "Circle", "Stabilize", @@ -241,179 +232,214 @@ static const char* flight_mode_strings[] = { // Failsafe // -------- -static int failsafe; // track which type of failsafe is being processed -static bool ch3_failsafe; -static byte crash_timer; +int failsafe; // track which type of failsafe is being processed +bool ch3_failsafe; +byte crash_timer; // Radio // ----- -static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm -static uint16_t elevon2_trim = 1500; -static uint16_t ch1_temp = 1500; // Used for elevon mixing -static uint16_t ch2_temp = 1500; -static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; -static bool rc_override_active = false; -static uint32_t rc_override_fs_timer = 0; -static uint32_t ch3_failsafe_timer = 0; +uint16_t elevon1_trim = 1500; // TODO: handle in EEProm +uint16_t elevon2_trim = 1500; +uint16_t ch1_temp = 1500; // Used for elevon mixing +uint16_t ch2_temp = 1500; +int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; +bool rc_override_active = false; +uint32_t rc_override_fs_timer = 0; +uint32_t ch3_failsafe_timer = 0; +bool reverse_roll; +bool reverse_pitch; +bool reverse_rudder; +byte mix_mode; // 0 = normal , 1 = elevons + +// TODO: switch these reverses to true/false, after they are handled by RC_Channel +int reverse_elevons = 1; +int reverse_ch1_elevon = 1; +int reverse_ch2_elevon = 1; // for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon // LED output // ---------- -static bool GPS_light; // status of the GPS light +bool GPS_light; // status of the GPS light // GPS variables // ------------- -static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage -static float scaleLongUp = 1; // used to reverse longitude scaling -static float scaleLongDown = 1; // used to reverse longitude scaling -static byte ground_start_count = 5; // have we achieved first lock and set Home? -static int ground_start_avg; // 5 samples to avg speed for ground start -static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present +const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage +float scaleLongUp = 1; // used to reverse longtitude scaling +float scaleLongDown = 1; // used to reverse longtitude scaling +byte ground_start_count = 5; // have we achieved first lock and set Home? +int ground_start_avg; // 5 samples to avg speed for ground start +bool ground_start; // have we started on the ground? +bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present // Location & Navigation // --------------------- const float radius_of_earth = 6378100; // meters const float gravity = 9.81; // meters/ sec^2 -static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate -static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target -static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate -static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? -static long hold_course = -1; // deg * 100 dir of plane +long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target +long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate +float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? +long hold_course = -1; // deg * 100 dir of plane -static byte command_must_index; // current command memory location -static byte command_may_index; // current command memory location -static byte command_must_ID; // current command ID -static byte command_may_ID; // current command ID +byte command_must_index; // current command memory location +byte command_may_index; // current command memory location +byte command_must_ID; // current command ID +byte command_may_ID; // current command ID // Airspeed // -------- -static int airspeed; // m/s * 100 -static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -static float airspeed_error; // m/s * 100 -static long energy_error; // energy state error (kinetic + potential) for altitude hold -static long airspeed_energy_error; // kinetic portion of energy error +int airspeed; // m/s * 100 +int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range +float airspeed_error; // m/s * 100 +long energy_error; // energy state error (kinetic + potential) for altitude hold +long airspeed_energy_error; // kinetic portion of energy error +bool airspeed_enabled = false; // Location Errors // --------------- -static long bearing_error; // deg * 100 : 0 to 36000 -static long altitude_error; // meters * 100 we are off in altitude -static float crosstrack_error; // meters we are off trackline +long bearing_error; // deg * 100 : 0 to 36000 +long altitude_error; // meters * 100 we are off in altitude +float crosstrack_error; // meters we are off trackline // Battery Sensors // --------------- -static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter -static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter -static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter +float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter +float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter +float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter +float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter +float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter -static float current_amps; -static float current_total; +float current_amps; +float current_total; // Airspeed Sensors // ---------------- -static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering -static int airspeed_pressure; // airspeed as a pressure value +float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering +int airspeed_offset; // analog air pressure sensor while still +int airspeed_pressure; // airspeed as a pressure value // Barometer Sensor variables // -------------------------- -static unsigned long abs_pressure; +unsigned long abs_pressure; // Altitude Sensor variables // ---------------------- -static int sonar_alt; +//byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR // flight mode specific // -------------------- -static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. -static bool land_complete; -static long takeoff_altitude; -// static int landing_distance; // meters; -static int landing_pitch; // pitch for landing set by commands -static int takeoff_pitch; +bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. +bool land_complete; +long takeoff_altitude; +int landing_distance; // meters; +int landing_pitch; // pitch for landing set by commands +int takeoff_pitch; // Loiter management // ----------------- -static long old_target_bearing; // deg * 100 -static int loiter_total; // deg : how many times to loiter * 360 -static int loiter_delta; // deg : how far we just turned -static int loiter_sum; // deg : how far we have turned around a waypoint -static long loiter_time; // millis : when we started LOITER mode -static int loiter_time_max; // millis : how long to stay in LOITER mode +long old_target_bearing; // deg * 100 +int loiter_total; // deg : how many times to loiter * 360 +int loiter_delta; // deg : how far we just turned +int loiter_sum; // deg : how far we have turned around a waypoint +long loiter_time; // millis : when we started LOITER mode +int loiter_time_max; // millis : how long to stay in LOITER mode // these are the values for navigation control functions // ---------------------------------------------------- -static long nav_roll; // deg * 100 : target roll angle -static long nav_pitch; // deg * 100 : target pitch angle -static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel +long nav_roll; // deg * 100 : target roll angle +long nav_pitch; // deg * 100 : target pitch angle +int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel // Waypoints // --------- -static long wp_distance; // meters - distance between plane and next waypoint -static long wp_totalDistance; // meters - distance between old and next waypoint +long wp_distance; // meters - distance between plane and next waypoint +long wp_totalDistance; // meters - distance between old and next waypoint +byte next_wp_index; // Current active command index // repeating event control // ----------------------- -static byte event_id; // what to do - see defines -static long event_timer; // when the event was asked for in ms -static uint16_t event_delay; // how long to delay the next firing of event in millis -static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles -static int event_value; // per command value, such as PWM for servos -static int event_undo_value; // the value used to cycle events (alternate value to event_value) +byte event_id; // what to do - see defines +long event_timer; // when the event was asked for in ms +uint16_t event_delay; // how long to delay the next firing of event in millis +int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles +int event_value; // per command value, such as PWM for servos +int event_undo_value; // the value used to cycle events (alternate value to event_value) // delay command // -------------- -static long condition_value; // used in condition commands (eg delay, change alt, etc.) -static long condition_start; -static int condition_rate; +long condition_value; // used in condition commands (eg delay, change alt, etc.) +long condition_start; +int condition_rate; // 3D Location vectors // ------------------- -static struct Location home; // home location -static struct Location prev_WP; // last waypoint -static struct Location current_loc; // current location -static struct Location next_WP; // next waypoint -static struct Location next_command; // command preloaded -static struct Location guided_WP; // guided mode waypoint -static long target_altitude; // used for altitude management between waypoints -static long offset_altitude; // used for altitude management between waypoints -static bool home_is_set; // Flag for if we have g_gps lock and have set the home location +struct Location home; // home location +struct Location prev_WP; // last waypoint +struct Location current_loc; // current location +struct Location next_WP; // next waypoint +struct Location next_command; // command preloaded +long target_altitude; // used for altitude management between waypoints +long offset_altitude; // used for altitude management between waypoints +bool home_is_set; // Flag for if we have g_gps lock and have set the home location // IMU variables // ------------- -static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) +float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) // Performance monitoring // ---------------------- -static long perf_mon_timer; // Metric based on accel gain deweighting -static int G_Dt_max = 0; // Max main loop cycle time in milliseconds -static int gps_fix_count = 0; -static int pmTest1 = 0; +long perf_mon_timer; // Metric based on accel gain deweighting +int G_Dt_max; // Max main loop cycle time in milliseconds +int gps_fix_count; +byte gcs_messages_sent; +// GCS +// --- +char GCS_buffer[53]; +char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed + // System Timers // -------------- -static unsigned long fast_loopTimer; // Time in miliseconds of main control loop -static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete -static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -static int mainLoop_count; +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; -static unsigned long medium_loopTimer; // Time in miliseconds of medium loop -static byte medium_loopCounter; // Counters for branching from main control loop to slower loops -static uint8_t delta_ms_medium_loop; +unsigned long medium_loopTimer; // Time in miliseconds of medium loop +byte medium_loopCounter; // Counters for branching from main control loop to slower loops +uint8_t delta_ms_medium_loop; -static byte slow_loopCounter; -static byte superslow_loopCounter; -static byte counter_one_herz; +byte slow_loopCounter; +byte superslow_loopCounter; +byte counter_one_herz; -static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav +unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav -static unsigned long dTnav; // Delta Time in milliseconds for navigation computations -static float load; // % MCU cycles used +unsigned long dTnav; // Delta Time in milliseconds for navigation computations +unsigned long elapsedTime; // for doing custom events +float load; // % MCU cycles used + +//Camera tracking and stabilisation stuff +// -------------------------------------- +byte camera_mode = 1; //0 is do nothing, 1 is stabilize, 2 is track target +byte gimbal_mode = 0; // 0 - pitch & roll, 1 - pitch and yaw (pan & tilt), 2 - pitch, roll and yaw (to be added) +struct Location camera_target; //point of iterest for the camera to track +Vector3 target_vector(0,0,1); //x, y, z to target before rotating to planes axis, values are in meters +float cam_pitch; +float cam_roll; +float cam_tilt; +float cam_pan; + +struct Location GPS_mark; // GPS POI for position based triggering +int picture_time = 0; // waypoint trigger variable +int thr_pic = 0; // timer variable for throttle_pic +int camtrig = 83; // PK6 chosen as it not near anything so safer for soldering +pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) //////////////////////////////////////////////////////////////////////////////// @@ -421,7 +447,6 @@ static float load; // % MCU cycles used //////////////////////////////////////////////////////////////////////////////// void setup() { - memcheck_init(); init_ardupilot(); } @@ -466,7 +491,7 @@ void loop() } // Main loop 50Hz -static void fast_loop() +void fast_loop() { // This is the fast loop - we want it to execute at 50Hz if possible // ----------------------------------------------------------------- @@ -477,22 +502,15 @@ static void fast_loop() // ---------- read_radio(); - // try to send any deferred messages if the serial port now has - // some space available - gcs.send_message(MSG_RETRY_DEFERRED); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.send_message(MSG_RETRY_DEFERRED); -#endif - // check for loss of control signal failsafe condition // ------------------------------------ check_short_failsafe(); // Read Airspeed // ------------- - if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { + if (airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { read_airspeed(); - } else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { + } else if (airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { calc_airspeed_errors(); } @@ -532,7 +550,7 @@ static void fast_loop() // write out the servo PWM values // ------------------------------ - set_servos(); + set_servos_4(); // XXX is it appropriate to be doing the comms below on the fast loop? @@ -561,8 +579,9 @@ static void fast_loop() // or be a "GCS fast loop" interface } -static void medium_loop() +void medium_loop() { + camera(); // This is the start of the medium (10 Hz) loop pieces // ----------------------------------------- switch(medium_loopCounter) { @@ -595,6 +614,7 @@ Serial.println(tempaccel.z, DEC); // This case performs some navigation computations //------------------------------------------------ case 1: + medium_loopCounter++; @@ -618,7 +638,6 @@ Serial.println(tempaccel.z, DEC); // Read altitude from sensors // ------------------ update_alt(); - if(g.sonar_enabled) sonar_alt = sonar.read(); // altitude smoothing // ------------------ @@ -679,7 +698,7 @@ Serial.println(tempaccel.z, DEC); } } -static void slow_loop() +void slow_loop() { // This is the slow (3 1/3 Hz) loop pieces //---------------------------------------- @@ -708,7 +727,9 @@ static void slow_loop() // Read Control Surfaces/Mix switches // ---------------------------------- - update_servo_switches(); + if (g.switch_enable) { + update_servo_switches(); + } break; @@ -716,18 +737,19 @@ static void slow_loop() slow_loopCounter = 0; update_events(); + // XXX this should be a "GCS slow loop" interface #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter - gcs.data_stream_send(3,5); + gcs.data_stream_send(1,5); // send all requested output streams with rates requested - // between 3 and 5 Hz + // between 1 and 5 Hz #else gcs.send_message(MSG_LOCATION); gcs.send_message(MSG_CPU_LOAD, load*100); #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(3,5); + hil.data_stream_send(1,5); #endif @@ -735,26 +757,20 @@ static void slow_loop() } } -static void one_second_loop() +void one_second_loop() { if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); // send a heartbeat gcs.send_message(MSG_HEARTBEAT); - #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(1,3); - // send all requested output streams with rates requested - // between 1 and 3 Hz - #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_HEARTBEAT); - hil.data_stream_send(1,3); #endif } -static void update_GPS(void) +void update_GPS(void) { g_gps->update(); update_GPS_light(); @@ -806,7 +822,7 @@ static void update_GPS(void) } } -static void update_current_flight_mode(void) +void update_current_flight_mode(void) { if(control_mode == AUTO){ crash_checker(); @@ -819,7 +835,7 @@ static void update_current_flight_mode(void) nav_roll = 0; } - if (g.airspeed_enabled == true) + if (airspeed_enabled == true) { calc_nav_pitch(); if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; @@ -837,7 +853,7 @@ static void update_current_flight_mode(void) case MAV_CMD_NAV_LAND: calc_nav_roll(); - if (g.airspeed_enabled == true){ + if (airspeed_enabled == true){ calc_nav_pitch(); calc_throttle(); }else{ @@ -862,7 +878,6 @@ static void update_current_flight_mode(void) switch(control_mode){ case RTL: case LOITER: - case GUIDED: hold_course = -1; crash_checker(); calc_nav_roll(); @@ -876,7 +891,6 @@ static void update_current_flight_mode(void) nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min; // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority - if (inverted_flight) nav_pitch = -nav_pitch; break; case FLY_BY_WIRE_B: @@ -886,7 +900,7 @@ static void update_current_flight_mode(void) nav_roll = g.channel_roll.norm_input() * g.roll_limit; altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; - if (g.airspeed_enabled == true) + if (airspeed_enabled == true) { airspeed_error = ((int)(g.flybywire_airspeed_max - g.flybywire_airspeed_min) * @@ -935,7 +949,7 @@ static void update_current_flight_mode(void) } } -static void update_navigation() +void update_navigation() { // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS // ------------------------------------------------------------------------ @@ -947,18 +961,23 @@ static void update_navigation() switch(control_mode){ case LOITER: - case RTL: - case GUIDED: update_loiter(); calc_bearing_error(); break; + case RTL: + if(wp_distance <= ( g.loiter_radius + LOITER_RANGE) ) { + do_RTL(); + }else{ + update_crosstrack(); + } + break; } } } -static void update_alt() +void update_alt() { #if HIL_MODE == HIL_MODE_ATTITUDE current_loc.alt = g_gps->altitude; @@ -971,6 +990,6 @@ static void update_alt() #endif // Calculate new climb rate - //if(medium_loopCounter == 0 && slow_loopCounter == 0) - // add_altitude_data(millis() / 100, g_gps->altitude / 10); + if(medium_loopCounter == 0 && slow_loopCounter == 0) + add_altitude_data(millis() / 100, g_gps->altitude / 10); } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 3305927fa4..b942ec759d 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -4,14 +4,15 @@ // Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. //**************************************************************** -static void stabilize() +void stabilize() { + static byte temp = 0; float ch1_inf = 1.0; float ch2_inf = 1.0; float ch4_inf = 1.0; float speed_scaler; - if (g.airspeed_enabled == true){ + if (airspeed_enabled == true){ if(airspeed > 0) speed_scaler = (STANDARD_SPEED * 100) / airspeed; else @@ -19,7 +20,7 @@ static void stabilize() speed_scaler = constrain(speed_scaler, 0.5, 2.0); } else { if (g.channel_throttle.servo_out > 0){ - speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root + speed_scaler = 0.5 + (THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root // Should maybe be to the 2/7 power, but we aren't goint to implement that... }else{ speed_scaler = 1.67; @@ -31,16 +32,6 @@ static void stabilize() nav_roll = 0; } - if (inverted_flight) { - // we want to fly upside down. We need to cope with wrap of - // the roll_sensor interfering with wrap of nav_roll, which - // would really confuse the PID code. The easiest way to - // handle this is to ensure both go in the same direction from - // zero - nav_roll += 18000; - if (dcm.roll_sensor < 0) nav_roll -= 36000; - } - // For Testing Only // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; // Serial.printf_P(PSTR(" roll_sensor ")); @@ -53,10 +44,6 @@ static void stabilize() fabs(dcm.roll_sensor * g.kff_pitch_compensation) + (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - (dcm.pitch_sensor - g.pitch_trim); - if (inverted_flight) { - // when flying upside down the elevator control is inverted - tempcalc = -tempcalc; - } g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); // Mix Stick input to allow users to override control surfaces @@ -112,7 +99,7 @@ static void stabilize() //#endif } -static void crash_checker() +void crash_checker() { if(dcm.pitch_sensor < -4500){ crash_timer = 255; @@ -122,9 +109,9 @@ static void crash_checker() } -static void calc_throttle() +void calc_throttle() { - if (g.airspeed_enabled == false) { + if (airspeed_enabled == false) { int throttle_target = g.throttle_cruise + throttle_nudge; // no airspeed sensor, we use nav pitch to determine the proper throttle output @@ -158,7 +145,7 @@ static void calc_throttle() // Yaw is separated into a function for future implementation of heading hold on rolling take-off // ---------------------------------------------------------------------------------------- -static void calc_nav_yaw(float speed_scaler) +void calc_nav_yaw(float speed_scaler) { #if HIL_MODE != HIL_MODE_ATTITUDE Vector3f temp = imu.get_accel(); @@ -173,11 +160,11 @@ static void calc_nav_yaw(float speed_scaler) } -static void calc_nav_pitch() +void calc_nav_pitch() { // Calculate the Pitch of the plane // -------------------------------- - if (g.airspeed_enabled == true) { + if (airspeed_enabled == true) { nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); } else { nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); @@ -188,7 +175,7 @@ static void calc_nav_pitch() #define YAW_DAMPENER 0 -static void calc_nav_roll() +void calc_nav_roll() { // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. @@ -232,22 +219,18 @@ float roll_slew_limit(float servo) /***************************************** * Throttle slew limit *****************************************/ -static void throttle_slew_limit() +/*float throttle_slew_limit(float throttle) { - static int last = 1000; - if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit - - float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000 -Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last); - g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp); - last = g.channel_throttle.radio_out; - } + static float last; + float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f); + last = throttle; + return temp; } - +*/ // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -static void reset_I(void) +void reset_I(void) { g.pidNavRoll.reset_I(); g.pidNavPitchAirspeed.reset_I(); @@ -259,13 +242,11 @@ static void reset_I(void) /***************************************** * Set the flight control servos based on the current calculated values *****************************************/ -static void set_servos(void) +void set_servos_4(void) { - int flapSpeedSource = 0; - if(control_mode == MANUAL){ // do a direct pass through of radio values - if (g.mix_mode == 0){ + if (mix_mode == 0){ g.channel_roll.radio_out = g.channel_roll.radio_in; g.channel_pitch.radio_out = g.channel_pitch.radio_in; } else { @@ -274,31 +255,21 @@ static void set_servos(void) } g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_rudder.radio_out = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in; - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in; } else { - if (g.mix_mode == 0) { + if (mix_mode == 0){ g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); g.channel_rudder.calc_pwm(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.servo_out = g.channel_roll.servo_out; - g.rc_5.calc_pwm(); - } - if (g.rc_6_funct == RC_6_FUNCT_AILERON) { - g.rc_6.servo_out = g.channel_roll.servo_out; - g.rc_6.calc_pwm(); - } }else{ /*Elevon mode*/ float ch1; float ch2; - ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out); + ch1 = reverse_elevons * (g.channel_pitch.servo_out - g.channel_roll.servo_out); ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out; - g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); - g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); + g.channel_roll.radio_out = elevon1_trim + (reverse_ch1_elevon * (ch1 * 500.0/ ROLL_SERVO_MAX)); + g.channel_pitch.radio_out = elevon2_trim + (reverse_ch2_elevon * (ch2 * 500.0/ PITCH_SERVO_MAX)); } #if THROTTLE_OUT == 0 @@ -310,61 +281,55 @@ static void set_servos(void) g.channel_throttle.calc_pwm(); + //Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938 + /* TO DO - fix this for RC_Channel library #if THROTTLE_REVERSE == 1 radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE]; #endif */ - - throttle_slew_limit(); } - - if(control_mode <= FLY_BY_WIRE_B) { - if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in; - if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in; - } else if (control_mode >= FLY_BY_WIRE_C) { - if (g.airspeed_enabled == true) { - flapSpeedSource = g.airspeed_cruise; - } else { - flapSpeedSource = g.throttle_cruise; - } - if ( flapSpeedSource > g.flap_1_speed) { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0; - } else if (flapSpeedSource > g.flap_2_speed) { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent; - } else { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent; - } - } - -#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS + + // send values to the PWM timers for output // ---------------------------------------- APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos - APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos - APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos -#endif } -static void demo_servos(byte i) { +void demo_servos(byte i) { while(i > 0){ gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); -#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS APM_RC.OutputCh(1, 1400); - mavlink_delay(400); + delay(400); APM_RC.OutputCh(1, 1600); - mavlink_delay(200); + delay(200); APM_RC.OutputCh(1, 1500); -#endif - mavlink_delay(400); + delay(400); i--; } } +int readOutputCh(unsigned char ch) +{ + int pwm; + switch(ch) + { + case 0: pwm = OCR5B; break; // ch0 + case 1: pwm = OCR5C; break; // ch1 + case 2: pwm = OCR1B; break; // ch2 + case 3: pwm = OCR1C; break; // ch3 + case 4: pwm = OCR4C; break; // ch4 + case 5: pwm = OCR4B; break; // ch5 + case 6: pwm = OCR3C; break; // ch6 + case 7: pwm = OCR3B; break; // ch7 + case 8: pwm = OCR5A; break; // ch8, PL3 + case 9: pwm = OCR1A; break; // ch9, PB5 + case 10: pwm = OCR3A; break; // ch10, PE3 + } + pwm >>= 1; // pwm / 2; + return pwm; +} diff --git a/ArduPlane/Cam_move.pde b/ArduPlane/Cam_move.pde new file mode 100644 index 0000000000..bc8ad0eac7 --- /dev/null +++ b/ArduPlane/Cam_move.pde @@ -0,0 +1,210 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if CAMERA == ENABLED + +void init_camera() +{ + g.rc_camera_pitch.set_angle(4500); // throw of servo? + g.rc_camera_pitch.radio_min = 1000; // lowest radio input + g.rc_camera_pitch.radio_trim = 1500; // middle radio input + g.rc_camera_pitch.radio_max = 2000; // highest radio input + + g.rc_camera_roll.set_angle(4500); + g.rc_camera_roll.radio_min = 1000; + g.rc_camera_roll.radio_trim = 1500; + g.rc_camera_roll.radio_max = 2000; + + + //use test target for now + camera_target = home; + +} + +void camera() +{ + //decide what happens to camera depending on camera mode + switch(camera_mode) + { + case 0: + //do nothing, i.e lock camera in place + break; + case 1: + //stabilize + target_vector.x=0; //east to west gives +tive value (i.e. longitude) + target_vector.y=0; //south to north gives +tive value (i.e. latitude) + target_vector.z=100; //downwards is +tive + camera_move(); + break; + case 2: + //track target + if(g_gps->fix) + { + target_vector=get_location_vector(¤t_loc,&camera_target); + camera_move(); + } + break; + } +} + +void camera_move() +{ + Matrix3f m = dcm.get_dcm_transposed(); + Vector3 targ = m*target_vector; //to do: find out notion of x y convention + + switch(gimbal_mode) + { + case 0: // pitch & roll + cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch + cam_roll = degrees(atan2(targ.y, targ.z)); //roll + // check_limits(pitch); + // check_limits(roll); + // camera_out(); + break; + + case 1: // pitch and yaw + cam_tilt = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; + cam_pan = 9000 + atan2(-targ.y, targ.x) * 5729.57795; + if (cam_pan < 0) cam_pan += 36000; + // check_limits(pitch); + // check_limits(yaw); + // camera_out(); + break; + + /* case 2: // pitch, roll & yaw - not started + float cam_ritch = 100; + float cam_yoll = 100; + float cam_paw = 100; + break; */ + } +} + +/* void check_limits(axis,variable) // Use servo definitions to calculate for all servo throws - TO DO +{ + // find limits of servo range in deg + track_pan_right = PAN_CENTER + (PAN_RANGE/2); + track_pan_left = track_pan_right + (360 - PAN_RANGE); + if (track_pan_left > 360){ + track_pan_left = track_pan_left - 360; + } + // check track_bearing is "safe" - not outside pan servo limits + // if the bearing lies in the servo dead zone change bearing to closet edge + if (track_bearing < track_pan_left && track_bearing > track_pan_right){ + track_oor_l = abs(track_bearing - track_pan_left); + track_oor_r = abs(track_bearing - track_pan_right); + if (track_oor_r > track_oor_l){ + track_bearing = track_pan_right; + } + if (track_oor_l > track_oor_r){ + track_bearing = track_pan_left; + } + } + // center bearing to cam_servo center + track_pan_deg = track_bearing - PAN_CENTER; + // make negative is left rotation + if (track_pan_deg > 180){ + track_pan_deg = (180 - (track_pan_deg - 180)) * -1; + } + +} */ + +void camera_out() +{ + switch(gimbal_mode) + { + case 0: // pitch & roll + g.rc_camera_pitch.servo_out = cam_pitch; + g.rc_camera_pitch.calc_pwm(); + g.rc_camera_roll.servo_out = cam_roll; + g.rc_camera_roll.calc_pwm(); + break; + + case 1: // pitch and yaw + g.rc_camera_pitch.servo_out = cam_tilt; + g.rc_camera_pitch.calc_pwm(); + g.rc_camera_roll.servo_out = cam_pan; // borrowing roll servo output for pan/yaw + g.rc_camera_roll.calc_pwm(); + break; + + /*case 2: // pitch, roll & yaw + g.rc_camera_pitch.servo_out = cam_ritch; + g.rc_camera_pitch.calc_pwm(); + + g.rc_camera_roll.servo_out = cam_yoll; + g.rc_camera_roll.calc_pwm(); + + g.rc_camera_yaw.servo_out = cam_paw; // camera_yaw doesn't exist it should unless we use another channel + g.rc_camera_yaw.calc_pwm(); + break; */ + } +#if defined PITCH_SERVO + APM_RC.OutputCh(PITCH_SERVO, g.rc_camera_pitch.radio_out); +#endif +#if defined ROLL_SERVO + APM_RC.OutputCh(ROLL_SERVO, g.rc_camera_roll.radio_out); +#endif +/*#if defined YAW_SERVO + APM_RC.OutputCh(YAW_SERVO, g.rc_camera_yaw.radio_out); +#endif */ + +#if CAM_DEBUG == ENABLED + //for debugging purposes + Serial.println(); + Serial.print("current_loc: lat: "); + Serial.print(current_loc.lat); + Serial.print(", lng: "); + Serial.print(current_loc.lng); + Serial.print(", alt: "); + Serial.print(current_loc.alt); + Serial.println(); + Serial.print("target_loc: lat: "); + Serial.print(camera_target.lat); + Serial.print(", lng: "); + Serial.print(camera_target.lng); + Serial.print(", alt: "); + Serial.print(camera_target.alt); + Serial.print(", distance: "); + Serial.print(get_distance(¤t_loc,&camera_target)); + Serial.print(", bearing: "); + Serial.print(get_bearing(¤t_loc,&camera_target)); + Serial.println(); + Serial.print("dcm_angles: roll: "); + Serial.print(degrees(dcm.roll)); + Serial.print(", pitch: "); + Serial.print(degrees(dcm.pitch)); + Serial.print(", yaw: "); + Serial.print(degrees(dcm.yaw)); + Serial.println(); + Serial.print("target_vector: x: "); + Serial.print(target_vector.x,2); + Serial.print(", y: "); + Serial.print(target_vector.y,2); + Serial.print(", z: "); + Serial.print(target_vector.z,2); + Serial.println(); + Serial.print("rotated_target_vector: x: "); + Serial.print(targ.x,2); + Serial.print(", y: "); + Serial.print(targ.y,2); + Serial.print(", z: "); + Serial.print(targ.z,2); + Serial.println(); + Serial.print("gimbal type 0: roll: "); + Serial.print(roll); + Serial.print(", pitch: "); + Serial.print(pitch); + Serial.println(); + /* Serial.print("gimbal type 1: pitch: "); + Serial.print(pan); + Serial.print(", roll: "); + Serial.print(tilt); + Serial.println(); */ + /* Serial.print("gimbal type 2: pitch: "); + Serial.print(ritch); + Serial.print(", roll: "); + Serial.print(yoll); + Serial.print(", yaw: "); + Serial.print(paw); + Serial.println(); */ +#endif +} +#endif diff --git a/ArduPlane/Cam_trigger.pde b/ArduPlane/Cam_trigger.pde new file mode 100644 index 0000000000..7b03a90c8b --- /dev/null +++ b/ArduPlane/Cam_trigger.pde @@ -0,0 +1,41 @@ +void servo_pic() // Servo operated camera +{ + APM_RC.OutputCh(CAM_SERVO,1500 + (333)); // Camera click, not enough - add more, wring way - put a minus before bracket number (-300) + delay(250); // delay + APM_RC.OutputCh(CAM_SERVO,1500); // Return servo to mid position +} + +void relay_picture() // basic relay activation +{ + relay_on(); + delay(250); // 0.25 seconds delay + relay_off(); +} + +void throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. +{ + g.channel_throttle.radio_out = g.throttle_min; + if (thr_pic = 10){ + servo_pic(); // triggering method + thr_pic = 0; + g.channel_throttle.radio_out = g.throttle_cruise; + } + thr_pic++; +} + +void distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. +{ + g.channel_throttle.radio_out = g.throttle_min; + if (wp_distance < 3){ + servo_pic(); // triggering method + g.channel_throttle.radio_out = g.throttle_cruise; + } +} + +void NPN_trigger() // hacked the circuit to run a transistor? use this trigger to send output. +{ + // To Do: Assign pin spare pin for output + digitalWrite(camtrig, HIGH); + delay(50); + digitalWrite(camtrig, LOW); +} diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index a654d823eb..a80673966e 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -6,7 +6,7 @@ #ifndef __GCS_H #define __GCS_H -#include +#include #include #include #include @@ -40,7 +40,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(FastSerial *port) { _port = port; } + void init(BetterStream *port) { _port = port; } /// Update GCS state. /// @@ -119,7 +119,7 @@ public: protected: /// The stream we are communicating over - FastSerial *_port; + BetterStream *_port; }; // @@ -139,7 +139,7 @@ class GCS_MAVLINK : public GCS_Class public: GCS_MAVLINK(AP_Var::Key key); void update(void); - void init(FastSerial *port); + void init(BetterStream *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const prog_char_t *str); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index db34c15389..b578aedcca 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -4,10 +4,6 @@ #include "Mavlink_Common.h" -// use this to prevent recursion during sensor init -static bool in_mavlink_delay; - - GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : packet_drops(0), @@ -18,21 +14,21 @@ waypoint_receive_timeout(1000), // 1 second // stream rates _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), - // AP_VAR //ref //index, default, name - streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), - streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), - streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), - streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), - streamRatePosition (&_group, 4, 0, PSTR("POSITION")), - streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), - streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), - streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) -{ +streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), +streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), +streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), +streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), +streamRatePosition (&_group, 4, 0, PSTR("POSITION")), +streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), +streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), +streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) + +{ } void -GCS_MAVLINK::init(FastSerial * port) +GCS_MAVLINK::init(BetterStream * port) { GCS_Class::init(port); if (port == &Serial) { // to split hil vs gcs @@ -57,6 +53,8 @@ GCS_MAVLINK::update(void) { uint8_t c = comm_receive_ch(chan); + + // Try to get a new message if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); } @@ -64,6 +62,11 @@ GCS_MAVLINK::update(void) // Update packet drops counter packet_drops += status.packet_rx_drop_count; + + + + + // send out queued params/ waypoints _queued_send(); @@ -84,55 +87,41 @@ void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { + if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax)) + send_message(MSG_RAW_IMU); - if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ - send_message(MSG_RAW_IMU1); - send_message(MSG_RAW_IMU2); - send_message(MSG_RAW_IMU3); - //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); - } - - if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { - send_message(MSG_EXTENDED_STATUS1); - send_message(MSG_EXTENDED_STATUS2); + if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) { + send_message(MSG_EXTENDED_STATUS); send_message(MSG_GPS_STATUS); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); - //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); } - if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { - // sent with GPS read + if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) { send_message(MSG_LOCATION); - //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); } - if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { + if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) { // This is used for HIL. Do not change without discussing with HIL maintainers send_message(MSG_SERVO_OUT); - //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); } - if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { + if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); - //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); } - if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info + if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info send_message(MSG_ATTITUDE); - //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); } - if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info + if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info send_message(MSG_VFR_HUD); - //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); } - if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ + if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){ // Available datastream - //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } @@ -140,7 +129,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) void GCS_MAVLINK::send_message(uint8_t id, uint32_t param) { - mavlink_send_message(chan,id, packet_drops); + mavlink_send_message(chan,id,param,packet_drops); } void @@ -169,9 +158,9 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { - struct Location tell_command = {}; // command for telemetry + struct Location tell_command; // command for telemetry static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...) - + switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: @@ -179,18 +168,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; int freq = 0; // packet frequency - if (packet.start_stop == 0) - freq = 0; // stop sending - else if (packet.start_stop == 1) - freq = packet.req_message_rate; // start sending - else - break; + if (packet.start_stop == 0) freq = 0; // stop sending + else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending + else break; switch(packet.req_stream_id){ @@ -204,7 +188,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) streamRateExtra2 = freq; streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. break; - case MAV_DATA_STREAM_RAW_SENSORS: streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly // we will not continue to broadcast raw sensor data at 50Hz. @@ -212,35 +195,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_DATA_STREAM_EXTENDED_STATUS: streamRateExtendedStatus.set_and_save(freq); break; - case MAV_DATA_STREAM_RC_CHANNELS: streamRateRCChannels.set_and_save(freq); break; - case MAV_DATA_STREAM_RAW_CONTROLLER: streamRateRawController.set_and_save(freq); break; - - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; - + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; case MAV_DATA_STREAM_POSITION: streamRatePosition.set_and_save(freq); break; - case MAV_DATA_STREAM_EXTRA1: streamRateExtra1.set_and_save(freq); break; - case MAV_DATA_STREAM_EXTRA2: streamRateExtra2.set_and_save(freq); break; - case MAV_DATA_STREAM_EXTRA3: streamRateExtra3.set_and_save(freq); break; - default: break; } @@ -253,11 +228,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_action_t packet; mavlink_msg_action_decode(msg, &packet); if (mavlink_check_target(packet.target,packet.target_component)) break; - + uint8_t result = 0; // do action - send_text_P(SEVERITY_LOW,PSTR("action received: ")); + send_text_P(SEVERITY_LOW,PSTR("action received: ")); //Serial.println(packet.action); switch(packet.action){ @@ -357,13 +332,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) default: break; } - + mavlink_msg_action_ack_send( chan, packet.action, result ); - + break; } @@ -372,38 +347,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg, &packet); - + switch(packet.mode){ case MAV_MODE_MANUAL: set_mode(MANUAL); break; - - case MAV_MODE_GUIDED: - set_mode(GUIDED); + + case MAV_MODE_GUIDED: //For future use break; - + case MAV_MODE_AUTO: if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); mav_nav = 255; break; - + case MAV_MODE_TEST1: set_mode(STABILIZE); break; - case MAV_MODE_TEST2: - if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A); - if(mav_nav == 2) set_mode(FLY_BY_WIRE_B); - //if(mav_nav == 3) set_mode(FLY_BY_WIRE_C); - mav_nav = 255; + set_mode(FLY_BY_WIRE_A); + break; + case MAV_MODE_TEST3: + set_mode(FLY_BY_WIRE_B); break; - } } - + case MAVLINK_MSG_ID_SET_NAV_MODE: { // decode @@ -413,7 +385,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mav_nav = packet.nav_mode; break; } - + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); @@ -421,8 +393,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_list_t packet; mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // Start sending waypoints mavlink_msg_waypoint_count_send( @@ -439,7 +410,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); @@ -451,35 +421,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_t packet; mavlink_msg_waypoint_request_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // send waypoint tell_command = get_wp_with_index(packet.seq); // set frame of waypoint uint8_t frame; - - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame } else { frame = MAV_FRAME_GLOBAL; // reference frame } - + float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) - - if (packet.seq == (uint16_t)g.waypoint_index) - current = 1; - + if (packet.seq == g.waypoint_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) - float x = 0, y = 0, z = 0; - if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { + if (tell_command.id < MAV_CMD_NAV_LAST) { // command needs scaling x = tell_command.lat/1.0e7; // local (x), global (latitude) y = tell_command.lng/1.0e7; // local (y), global (longitude) @@ -489,63 +452,50 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) } } + - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_CONDITION_CHANGE_ALT: + case MAV_CMD_DO_SET_HOME: + param1 = tell_command.p1; + break; - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - param1 = tell_command.p1; - break; + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1*10; // APM loiter time is in ten second increments + break; - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1*10; // APM loiter time is in ten second increments - break; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; - case MAV_CMD_CONDITION_CHANGE_ALT: - x=0; // Clear fields loaded above that we don't want sent for this command - y=0; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - param1 = tell_command.p1; - break; + case MAV_CMD_DO_REPEAT_SERVO: + param4 = tell_command.lng; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + param3 = tell_command.lat; + param2 = tell_command.alt; + param1 = tell_command.p1; + break; - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - param3 = tell_command.lat; - param2 = tell_command.alt; - param1 = tell_command.p1; - break; + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + } - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - } - - mavlink_msg_waypoint_send(chan,msg->sysid, - msg->compid, - packet.seq, - frame, - tell_command.id, - current, - autocontinue, - param1, - param2, - param3, - param4, - x, - y, - z); + mavlink_msg_waypoint_send(chan,msg->sysid, + msg->compid,packet.seq,frame,tell_command.id,current,autocontinue, + param1,param2,param3,param4,x,y,z); // update last waypoint comm stamp waypoint_timelast_send = millis(); @@ -561,6 +511,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_ack_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; + // check for error + uint8_t type = packet.type; // ok (0), error(1) + // turn off waypoint send waypoint_sending = false; break; @@ -591,15 +544,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_clear_all_t packet; mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // clear all waypoints uint8_t type = 0; // ok (0), error(1) g.waypoint_total.set_and_save(0); // send acknowledgement 3 times to makes sure it is received - for (int i=0;i<3;i++) - mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type); + for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); break; } @@ -634,7 +586,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) packet.count = MAX_WAYPOINTS; } g.waypoint_total.set_and_save(packet.count - 1); - waypoint_timelast_receive = millis(); waypoint_receiving = true; waypoint_sending = false; @@ -642,58 +593,55 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } -#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS - case MAVLINK_MSG_ID_SET_MAG_OFFSETS: - { - mavlink_set_mag_offsets_t packet; - mavlink_msg_set_mag_offsets_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); - break; - } -#endif - - // XXX receive a WP from GCS and store in EEPROM case MAVLINK_MSG_ID_WAYPOINT: { + // Check if receiving waypiont + if (!waypoint_receiving) break; + // decode mavlink_waypoint_t packet; mavlink_msg_waypoint_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) break; + + // store waypoint + uint8_t loadAction = 0; // 0 insert in list, 1 exec now + // defaults tell_command.id = packet.command; - switch (packet.frame) - { - case MAV_FRAME_MISSION: - case MAV_FRAME_GLOBAL: - { - 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 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm - tell_command.options = 0; // absolute altitude - break; - } + switch (packet.frame) + { + case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: + { + 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 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.options = 0; + break; + } - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; - break; - } - case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - { - 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 - tell_command.alt = packet.z * 1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! - break; - } - } + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2; + tell_command.options = 1; + break; + } + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + { + 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 + tell_command.alt = packet.z*1.0e2; + tell_command.options = 1; + break; + } + } switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields case MAV_CMD_NAV_LOITER_TURNS: @@ -703,7 +651,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.lat = packet.param1; + tell_command.p1 = packet.param1 * 100; break; case MAV_CMD_NAV_LOITER_TIME: @@ -737,53 +685,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission - guided_WP = tell_command; + set_wp_with_index(tell_command, packet.seq); - // add home alt if needed - if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ - guided_WP.alt += home.alt; - } + // update waypoint receiving state machine + waypoint_timelast_receive = millis(); + waypoint_request_i++; - set_mode(GUIDED); + if (waypoint_request_i > g.waypoint_total) + { + uint8_t type = 0; // ok (0), error(1) - // make any new wp uploaded instant (in case we are already in Guided mode) - set_guided_WP(); + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + type); - // verify we recevied the command - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - 0); - - } else { - // Check if receiving waypoints (mission upload expected) - if (!waypoint_receiving) break; - - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) break; - set_wp_with_index(tell_command, packet.seq); - - // update waypoint receiving state machine - waypoint_timelast_receive = millis(); - waypoint_request_i++; - - if (waypoint_request_i > (uint16_t)g.waypoint_total){ - uint8_t type = 0; // ok (0), error(1) - - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - type); - - send_text_P(SEVERITY_LOW,PSTR("flight plan received")); - waypoint_receiving = false; - // XXX ignores waypoint radius for individual waypoints, can - // only set WP_RADIUS parameter - } - } + send_text_P(SEVERITY_LOW,PSTR("flight plan received")); + waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } break; } @@ -795,9 +717,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_param_set_t packet; mavlink_msg_param_set_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // set parameter @@ -814,7 +734,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // add a small amount before casting parameter values // from float to integer to avoid truncating to the // next lower integer value. - float rounding_addition = 0.01; + const float rounding_addition = 0.01; // fetch the variable type ID var_type = vp->meta_type_id(); @@ -827,13 +747,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ((AP_Float16 *)vp)->set_and_save(packet.param_value); } else if (var_type == AP_Var::k_typeid_int32) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int16) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int8) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); } else { // we don't support mavlink set on this parameter @@ -864,10 +783,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system,packet.target_component)) - break; - + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; @@ -880,20 +797,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) rc_override_fs_timer = millis(); break; } - + case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if(msg->sysid != g.sysid_my_gcs) break; rc_override_fs_timer = millis(); - pmTest1++; break; } - #if HIL_MODE != HIL_MODE_DISABLED +#if HIL_MODE != HIL_MODE_DISABLED // This is used both as a sensor and to pass the location // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: + case MAVLINK_MSG_ID_GPS_RAW: { // decode mavlink_gps_raw_t packet; @@ -1035,7 +951,7 @@ void GCS_MAVLINK::_queued_send() { // Check to see if we are sending parameters - if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) { + if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) { AP_Var *vp; float value; @@ -1068,10 +984,9 @@ GCS_MAVLINK::_queued_send() // request waypoints one by one // XXX note that this is pan-interface if (waypoint_receiving && - (requested_interface == (unsigned)chan) && - waypoint_request_i <= (unsigned)g.waypoint_total && + (requested_interface == chan) && + waypoint_request_i <= g.waypoint_total && mavdelay > 15) { // limits to 3.33 hz - mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, @@ -1082,66 +997,5 @@ GCS_MAVLINK::_queued_send() } } -#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - -static void send_rate(uint16_t low, uint16_t high) { -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(low, high); #endif -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(low,high); -#endif -} -/* - a delay() callback that processes MAVLink packets. We set this as the - callback in long running library initialisation routines to allow - MAVLink to process packets while waiting for the initialisation to - complete -*/ -static void mavlink_delay(unsigned long t) -{ - unsigned long tstart; - static unsigned long last_1hz, last_3hz, last_10hz, last_50hz; - - if (in_mavlink_delay) { - // this should never happen, but let's not tempt fate by - // letting the stack grow too much - delay(t); - return; - } - - in_mavlink_delay = true; - - tstart = millis(); - do { - unsigned long tnow = millis(); - if (tnow - last_1hz > 1000) { - last_1hz = tnow; - gcs.send_message(MSG_HEARTBEAT); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.send_message(MSG_HEARTBEAT); -#endif - send_rate(1, 3); - } - if (tnow - last_3hz > 333) { - last_3hz = tnow; - send_rate(3, 5); - } - if (tnow - last_10hz > 100) { - last_10hz = tnow; - send_rate(5, 45); - } - if (tnow - last_50hz > 20) { - last_50hz = tnow; - gcs.update(); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.update(); -#endif - send_rate(45, 1000); - } - delay(1); - } while (millis() - tstart < t); - - in_mavlink_delay = false; -} diff --git a/ArduPlane/HIL.h b/ArduPlane/HIL.h index 70ad73d6de..85b65fa1be 100644 --- a/ArduPlane/HIL.h +++ b/ArduPlane/HIL.h @@ -8,7 +8,7 @@ # if HIL_MODE != HIL_MODE_DISABLED -#include +#include #include #include #include @@ -41,7 +41,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(FastSerial *port) { _port = port; } + void init(BetterStream *port) { _port = port; } /// Update HIL state. /// @@ -83,7 +83,7 @@ public: protected: /// The stream we are communicating over - FastSerial *_port; + BetterStream *_port; }; // @@ -111,7 +111,7 @@ class HIL_XPLANE : public HIL_Class public: HIL_XPLANE(); void update(void); - void init(FastSerial *port); + void init(BetterStream *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const prog_char_t *str); diff --git a/ArduPlane/HIL_Xplane.pde b/ArduPlane/HIL_Xplane.pde index 4f01c19028..89721b6158 100644 --- a/ArduPlane/HIL_Xplane.pde +++ b/ArduPlane/HIL_Xplane.pde @@ -55,7 +55,7 @@ HIL_XPLANE::HIL_XPLANE() : } void -HIL_XPLANE::init(FastSerial * port) +HIL_XPLANE::init(BetterStream * port) { HIL_Class::init(port); hilPacketDecoder = new AP_GPS_IMU(port); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 59a9c66b0b..0157bd1727 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -1,7 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if LOGGING_ENABLED == ENABLED - // Code to Write and Read packets from DataFlash log memory // Code to interact with the user to dump or erase logs @@ -12,6 +10,7 @@ // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. +static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv); static int8_t dump_log(uint8_t argc, const Menu::arg *argv); static int8_t erase_logs(uint8_t argc, const Menu::arg *argv); static int8_t select_logs(uint8_t argc, const Menu::arg *argv); @@ -28,14 +27,13 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv) " enable |all enable logging or everything\n" " disable |all disable logging or everything\n" "\n")); - return 0; } // Creates a constant array of structs representing menu options // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Coommon for implementation details -static const struct Menu::command log_menu_commands[] PROGMEM = { +const struct Menu::command log_menu_commands[] PROGMEM = { {"dump", dump_log}, {"erase", erase_logs}, {"enable", select_logs}, @@ -46,8 +44,6 @@ static const struct Menu::command log_menu_commands[] PROGMEM = { // A Macro to create the Menu MENU2(log_menu, "Log", log_menu_commands, print_log_menu); -static void get_log_boundaries(byte log_num, int & start_page, int & end_page); - static bool print_log_menu(void) { @@ -84,7 +80,7 @@ print_log_menu(void) Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); for(int i=1;i 0) { for(int i=0;i 7){ logvar = DataFlash.ReadInt(); }else{ @@ -559,7 +550,7 @@ static void Log_Read_Performance() } // Read a command processing packet -static void Log_Read_Cmd() +void Log_Read_Cmd() { byte logvarb; long logvarl; @@ -578,7 +569,7 @@ static void Log_Read_Cmd() Serial.println(" "); } -static void Log_Read_Startup() +void Log_Read_Startup() { byte logbyte = DataFlash.ReadByte(); @@ -593,7 +584,7 @@ static void Log_Read_Startup() } // Read an attitude packet -static void Log_Read_Attitude() +void Log_Read_Attitude() { Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), DataFlash.ReadInt(), @@ -602,22 +593,21 @@ static void Log_Read_Attitude() } // Read a mode packet -static void Log_Read_Mode() +void Log_Read_Mode() { Serial.printf_P(PSTR("MOD:")); Serial.println(flight_mode_strings[DataFlash.ReadByte()]); } // Read a GPS packet -static void Log_Read_GPS() +void Log_Read_GPS() { - Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"), + Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"), DataFlash.ReadLong(), (int)DataFlash.ReadByte(), (int)DataFlash.ReadByte(), (float)DataFlash.ReadLong() / t7, (float)DataFlash.ReadLong() / t7, - (float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing (float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0, @@ -626,7 +616,7 @@ static void Log_Read_GPS() } // Read a raw accel/gyro packet -static void Log_Read_Raw() +void Log_Read_Raw() { float logvar; Serial.printf_P(PSTR("RAW:")); @@ -639,19 +629,13 @@ static void Log_Read_Raw() } // Read the DataFlash log memory : Packet Parser -static void Log_Read(int start_page, int end_page) +void Log_Read(int start_page, int end_page) { byte data; byte log_step = 0; int packet_count = 0; int page = start_page; - #ifdef AIRFRAME_NAME - Serial.printf_P(PSTR((AIRFRAME_NAME) - #endif - Serial.printf_P(PSTR("\n" THISFIRMWARE - "\nFree RAM: %lu\n"), - freeRAM()); DataFlash.StartRead(start_page); while (page < end_page && page != -1){ @@ -728,23 +712,4 @@ static void Log_Read(int start_page, int end_page) Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); } -#else // LOGGING_ENABLED -// dummy functions -static void Log_Write_Mode(byte mode) {} -static void Log_Write_Startup(byte type) {} -static void Log_Write_Cmd(byte num, struct Location *wp) {} -static void Log_Write_Current() {} -static void Log_Write_Nav_Tuning() {} -static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, - long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {} -static void Log_Write_Performance() {} -static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -static byte get_num_logs(void) { return 0; } -static void start_new_log(byte num_existing_logs) {} -static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {} -static void Log_Write_Control_Tuning() {} -static void Log_Write_Raw() {} - - -#endif // LOGGING_ENABLED diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..6c701c47df 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -25,36 +25,21 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } -// try to send a message, return false if it won't fit in the serial tx buffer -static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) + +void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) { uint64_t timeStamp = micros(); - int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; - -#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false - - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // defer any messages on the telemetry port for 1 second after - // bootup, to try to prevent bricking of Xbees - return false; - } - switch(id) { case MSG_HEARTBEAT: - { - CHECK_PAYLOAD_SIZE(HEARTBEAT); mavlink_msg_heartbeat_send( chan, mavlink_system.type, MAV_AUTOPILOT_ARDUPILOTMEGA); break; - } - case MSG_EXTENDED_STATUS1: + case MSG_EXTENDED_STATUS: { - CHECK_PAYLOAD_SIZE(SYS_STATUS); - uint8_t mode = MAV_MODE_UNINIT; uint8_t nav_mode = MAV_NAV_VECTOR; @@ -67,14 +52,9 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; case FLY_BY_WIRE_A: mode = MAV_MODE_TEST2; - nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. break; case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST2; - nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case GUIDED: - mode = MAV_MODE_GUIDED; + mode = MAV_MODE_TEST3; break; case AUTO: mode = MAV_MODE_AUTO; @@ -88,10 +68,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ mode = MAV_MODE_AUTO; nav_mode = MAV_NAV_LOITER; break; - case INITIALISING: - mode = MAV_MODE_UNINIT; - nav_mode = MAV_NAV_GROUNDED; - break; } uint8_t status = MAV_STATE_ACTIVE; @@ -106,20 +82,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ battery_voltage * 1000, battery_remaining, packet_drops); - break; - } - - case MSG_EXTENDED_STATUS2: - { - CHECK_PAYLOAD_SIZE(MEMINFO); - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); break; - } + } case MSG_ATTITUDE: { - CHECK_PAYLOAD_SIZE(ATTITUDE); Vector3f omega = dcm.get_gyro(); mavlink_msg_attitude_send( chan, @@ -135,7 +102,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_LOCATION: { - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_global_position_int_send( chan, @@ -151,7 +117,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_NAV_CONTROLLER_OUTPUT: { if (control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); mavlink_msg_nav_controller_output_send( chan, nav_roll / 1.0e2, @@ -168,7 +133,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_LOCAL_LOCATION: { - CHECK_PAYLOAD_SIZE(LOCAL_POSITION); Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_local_position_send( chan, @@ -184,7 +148,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_GPS_RAW: { - CHECK_PAYLOAD_SIZE(GPS_RAW); mavlink_msg_gps_raw_send( chan, timeStamp, @@ -201,7 +164,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_SERVO_OUT: { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); uint8_t rssi = 1; // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with HIL maintainers @@ -221,7 +183,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_RADIO_IN: { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); uint8_t rssi = 1; mavlink_msg_rc_channels_raw_send( chan, @@ -239,7 +200,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_RADIO_OUT: { - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); mavlink_msg_servo_output_raw_send( chan, g.channel_roll.radio_out, @@ -255,7 +215,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_VFR_HUD: { - CHECK_PAYLOAD_SIZE(VFR_HUD); mavlink_msg_vfr_hud_send( chan, (float)airspeed / 100.0, @@ -268,12 +227,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } #if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU1: + case MSG_RAW_IMU: { - CHECK_PAYLOAD_SIZE(RAW_IMU); Vector3f accel = imu.get_accel(); Vector3f gyro = imu.get_gyro(); - //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); mavlink_msg_raw_imu_send( @@ -288,42 +245,21 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ compass.mag_x, compass.mag_y, compass.mag_z); - break; - } - case MSG_RAW_IMU2: - { - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + /* This message is not working. Why? mavlink_msg_scaled_pressure_send( - chan, - timeStamp, - (float)barometer.Press/100.0, - (float)(barometer.Press-g.ground_pressure)/100.0, - (int)(barometer.Temp*10)); - break; - } - - case MSG_RAW_IMU3: - { - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - Vector3f mag_offsets = compass.get_offsets(); - - mavlink_msg_sensor_offsets_send(chan, - mag_offsets.x, - mag_offsets.y, - mag_offsets.z, - compass.get_declination(), - barometer.RawPress, - barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), - imu.ax(), imu.ay(), imu.az()); + chan, + timeStamp, + (float)barometer.Press/100., + (float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw + (int)(barometer.Temp*100)); + */ break; } #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE case MSG_GPS_STATUS: { - CHECK_PAYLOAD_SIZE(GPS_STATUS); mavlink_msg_gps_status_send( chan, g_gps->num_sats, @@ -337,7 +273,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_CURRENT_WAYPOINT: { - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); mavlink_msg_waypoint_current_send( chan, g.waypoint_index); @@ -347,77 +282,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ default: break; } - return true; -} - - -#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of - // switch types in mavlink_try_send_message() -static struct mavlink_queue { - uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; - uint8_t next_deferred_message; - uint8_t num_deferred_messages; -} mavlink_queue[2]; - -// send a message using mavlink -static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) -{ - uint8_t i, nextid; - struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; - - // see if we can send the deferred messages, if any - while (q->num_deferred_messages != 0) { - if (!mavlink_try_send_message(chan, - q->deferred_messages[q->next_deferred_message], - packet_drops)) { - break; - } - q->next_deferred_message++; - if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { - q->next_deferred_message = 0; - } - q->num_deferred_messages--; - } - - if (id == MSG_RETRY_DEFERRED) { - return; - } - - // this message id might already be deferred - for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { - if (q->deferred_messages[nextid] == id) { - // its already deferred, discard - return; - } - nextid++; - if (nextid == MAX_DEFERRED_MESSAGES) { - nextid = 0; - } - } - - if (q->num_deferred_messages != 0 || - !mavlink_try_send_message(chan, id, packet_drops)) { - // can't send it now, so defer it - if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { - // the defer buffer is full, discard - return; - } - nextid = q->next_deferred_message + q->num_deferred_messages; - if (nextid >= MAX_DEFERRED_MESSAGES) { - nextid -= MAX_DEFERRED_MESSAGES; - } - q->deferred_messages[nextid] = id; - q->num_deferred_messages++; - } } void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str) { - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // don't send status MAVLink messages for 1 second after - // bootup, to try to prevent Xbee bricking - return; - } mavlink_msg_statustext_send( chan, severity, diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 570c38800a..8d8001b124 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -1,465 +1,381 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef PARAMETERS_H -#define PARAMETERS_H - -#include - -// Global parameter class. -// -class Parameters { -public: - // The version of the layout as described by the parameter enum. - // - // When changing the parameter enum in an incompatible fashion, this - // value should be incremented by one. - // - // The increment will prevent old parameters from being used incorrectly - // by newer code. - // - static const uint16_t k_format_version = 11; - - // The parameter software_type is set up solely for ground station use - // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) - // GCS will interpret values 0-9 as ArduPilotMega. Developers may use - // values within that range to identify different branches. - // - static const uint16_t k_software_type = 0; // 0 for APM trunk - - // - // Parameter identities. - // - // The enumeration defined here is used to ensure that every parameter - // or parameter group has a unique ID number. This number is used by - // AP_Var to store and locate parameters in EEPROM. - // - // Note that entries without a number are assigned the next number after - // the entry preceding them. When adding new entries, ensure that they - // don't overlap. - // - // Try to group related variables together, and assign them a set - // range in the enumeration. Place these groups in numerical order - // at the end of the enumeration. - // - // WARNING: Care should be taken when editing this enumeration as the - // AP_Var load/save code depends on the values here to identify - // variables saved in EEPROM. - // - // - enum { - // Layout version number, always key zero. - // - k_param_format_version = 0, - k_param_software_type, - - - // Misc - // - - k_param_auto_trim, - k_param_switch_enable, - k_param_log_bitmask, - k_param_pitch_trim, - k_param_mix_mode, - k_param_reverse_elevons, - k_param_reverse_ch1_elevon, - k_param_reverse_ch2_elevon, - k_param_flap_1_percent, - k_param_flap_1_speed, - k_param_flap_2_percent, - k_param_flap_2_speed, - k_param_num_resets, - - - // 110: Telemetry control - // - k_param_streamrates_port0 = 110, - k_param_streamrates_port3, - k_param_sysid_this_mav, - k_param_sysid_my_gcs, - k_param_serial3_baud, - - // 120: Fly-by-wire control - // - k_param_flybywire_airspeed_min = 120, - k_param_flybywire_airspeed_max, - - // - // 130: Sensor parameters - // - k_param_IMU_calibration = 130, - k_param_altitude_mix, - k_param_airspeed_ratio, - k_param_ground_temperature, - k_param_ground_pressure, - k_param_compass_enabled, - k_param_compass, - k_param_battery_monitoring, - k_param_pack_capacity, - k_param_airspeed_offset, - k_param_sonar_enabled, - k_param_airspeed_enabled, - - // - // 150: Navigation parameters - // - k_param_crosstrack_gain = 150, - k_param_crosstrack_entry_angle, - k_param_roll_limit, - k_param_pitch_limit_max, - k_param_pitch_limit_min, - k_param_airspeed_cruise, - k_param_RTL_altitude, - k_param_inverted_flight_ch, - - // - // 170: Radio settings - // - k_param_channel_roll = 170, - k_param_channel_pitch, - k_param_channel_throttle, - k_param_channel_yaw, - k_param_rc_5, - k_param_rc_6, - k_param_rc_7, - k_param_rc_8, - - k_param_throttle_min, - k_param_throttle_max, - k_param_throttle_fs_enabled, - k_param_throttle_fs_value, - k_param_throttle_cruise, - - k_param_short_fs_action, - k_param_long_fs_action, - k_param_gcs_heartbeat_fs_enabled, - k_param_throttle_slewrate, - - k_param_rc_5_funct, - k_param_rc_6_funct, - k_param_rc_7_funct, - k_param_rc_8_funct, - - // - // 200: Feed-forward gains - // - k_param_kff_pitch_compensation = 200, - k_param_kff_rudder_mix, - k_param_kff_pitch_to_throttle, - k_param_kff_throttle_to_pitch, - - // - // 210: flight modes - // - k_param_flight_mode_channel = 210, - k_param_flight_mode1, - k_param_flight_mode2, - k_param_flight_mode3, - k_param_flight_mode4, - k_param_flight_mode5, - k_param_flight_mode6, - - // - // 220: Waypoint data - // - k_param_waypoint_mode = 220, - k_param_waypoint_total, - k_param_waypoint_index, - k_param_waypoint_radius, - k_param_loiter_radius, - - // - // 240: PID Controllers - // - // Heading-to-roll PID: - // heading error from command to roll command deviation from trim - // (bank to turn strategy) - // - k_param_heading_to_roll_PID = 240, - - // Roll-to-servo PID: - // roll error from command to roll servo deviation from trim - // (tracks commanded bank angle) - // - k_param_roll_to_servo_PID, - - // - // Pitch control - // - // Pitch-to-servo PID: - // pitch error from command to pitch servo deviation from trim - // (front-side strategy) - // - k_param_pitch_to_servo_PID, - - // Airspeed-to-pitch PID: - // airspeed error from command to pitch servo deviation from trim - // (back-side strategy) - // - k_param_airspeed_to_pitch_PID, - - // - // Yaw control - // - // Yaw-to-servo PID: - // yaw rate error from command to yaw servo deviation from trim - // (stabilizes dutch roll) - // - k_param_yaw_to_servo_PID, - - // - // Throttle control - // - // Energy-to-throttle PID: - // total energy error from command to throttle servo deviation from trim - // (throttle back-side strategy alternative) - // - k_param_energy_to_throttle_PID, - - // Altitude-to-pitch PID: - // altitude error from command to pitch servo deviation from trim - // (throttle front-side strategy alternative) - // - k_param_altitude_to_pitch_PID, - - // 254,255: reserved - }; - - AP_Int16 format_version; - AP_Int8 software_type; - - // Telemetry control - // - AP_Int16 sysid_this_mav; - AP_Int16 sysid_my_gcs; - AP_Int8 serial3_baud; - - // Feed-forward gains - // - AP_Float kff_pitch_compensation; - AP_Float kff_rudder_mix; - AP_Float kff_pitch_to_throttle; - AP_Float kff_throttle_to_pitch; - - // Crosstrack navigation - // - AP_Float crosstrack_gain; - AP_Int16 crosstrack_entry_angle; - - // Estimation - // - AP_Float altitude_mix; - AP_Float airspeed_ratio; - AP_Int16 airspeed_offset; - - // Waypoints - // - AP_Int8 waypoint_mode; - AP_Int8 waypoint_total; - AP_Int8 waypoint_index; - AP_Int8 waypoint_radius; - AP_Int8 loiter_radius; - - // Fly-by-wire - // - AP_Int8 flybywire_airspeed_min; - AP_Int8 flybywire_airspeed_max; - - // Throttle - // - AP_Int8 throttle_min; - AP_Int8 throttle_max; - AP_Int8 throttle_slewrate; - AP_Int8 throttle_fs_enabled; - AP_Int16 throttle_fs_value; - AP_Int8 throttle_cruise; - - // Failsafe - AP_Int8 short_fs_action; - AP_Int8 long_fs_action; - AP_Int8 gcs_heartbeat_fs_enabled; - - // Flight modes - // - AP_Int8 flight_mode_channel; - AP_Int8 flight_mode1; - AP_Int8 flight_mode2; - AP_Int8 flight_mode3; - AP_Int8 flight_mode4; - AP_Int8 flight_mode5; - AP_Int8 flight_mode6; - - // Navigational maneuvering limits - // - AP_Int16 roll_limit; - AP_Int16 pitch_limit_max; - AP_Int16 pitch_limit_min; - - // Misc - // - AP_Int8 auto_trim; - AP_Int8 switch_enable; - AP_Int8 mix_mode; - AP_Int8 reverse_elevons; - AP_Int8 reverse_ch1_elevon; - AP_Int8 reverse_ch2_elevon; - AP_Int16 num_resets; - AP_Int16 log_bitmask; - AP_Int16 airspeed_cruise; - AP_Int16 pitch_trim; - AP_Int16 RTL_altitude; - AP_Int16 ground_temperature; - AP_Int32 ground_pressure; - AP_Int8 compass_enabled; - AP_Int16 angle_of_attack; - AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current - AP_Int16 pack_capacity; // Battery pack capacity less reserve - AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger - AP_Int8 sonar_enabled; - AP_Int8 airspeed_enabled; - AP_Int8 flap_1_percent; - AP_Int8 flap_1_speed; - AP_Int8 flap_2_percent; - AP_Int8 flap_2_speed; - - // RC channels - RC_Channel channel_roll; - RC_Channel channel_pitch; - RC_Channel channel_throttle; - RC_Channel channel_rudder; - RC_Channel rc_5; - RC_Channel rc_6; - RC_Channel rc_7; - RC_Channel rc_8; - AP_Int8 rc_5_funct; - AP_Int8 rc_6_funct; - AP_Int8 rc_7_funct; - AP_Int8 rc_8_funct; - - // PID controllers - // - PID pidNavRoll; - PID pidServoRoll; - PID pidServoPitch; - PID pidNavPitchAirspeed; - PID pidServoRudder; - PID pidTeThrottle; - PID pidNavPitchAltitude; - - uint8_t junk; - - // Note: keep initializers here in the same order as they are declared above. - Parameters() : - // variable default key name - //------------------------------------------------------------------------------------------------------- - format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")), - software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")), - - sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), - sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), - serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")), - - kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")), - kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")), - kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")), - kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")), - - crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), - crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), - - altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")), - airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")), - airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")), - - /* XXX waypoint_mode missing here */ - waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), - waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), - waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), - loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), - - flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")), - flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")), - - throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), - throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), - throttle_slewrate (THROTTLE_SLEW_LIMIT, k_param_throttle_slewrate, PSTR("THR_SLEWRATE")), - throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), - throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), - throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), - - short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), - long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), - gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), - - flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")), - flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")), - flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")), - flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")), - flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")), - flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")), - flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")), - - roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")), - pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")), - pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")), - - auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")), - switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")), - mix_mode (ELEVON_MIXING, k_param_mix_mode, PSTR("ELEVON_MIXING")), - reverse_elevons (ELEVON_REVERSE, k_param_reverse_elevons, PSTR("ELEVON_REVERSE")), - reverse_ch1_elevon (ELEVON_CH1_REVERSE, k_param_reverse_ch1_elevon, PSTR("ELEVON_CH1_REV")), - reverse_ch2_elevon (ELEVON_CH2_REVERSE, k_param_reverse_ch2_elevon, PSTR("ELEVON_CH2_REV")), - num_resets (0, k_param_num_resets, PSTR("SYS_NUM_RESETS")), - log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), - airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), - pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), - RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), - ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), - ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), - compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), - flap_1_percent (FLAP_1_PERCENT, k_param_flap_1_percent, PSTR("FLAP_1_PERCNT")), - flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")), - flap_2_percent (FLAP_2_PERCENT, k_param_flap_2_percent, PSTR("FLAP_2_PERCNT")), - flap_2_speed (FLAP_2_SPEED, k_param_flap_2_speed, PSTR("FLAP_2_SPEED")), - - - battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), - pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), - inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), - sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")), - airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")), - rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")), - rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")), - rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")), - rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")), - - // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! - - // RC channel group key name - //---------------------------------------------------------------------- - channel_roll (k_param_channel_roll, PSTR("RC1_")), - channel_pitch (k_param_channel_pitch, PSTR("RC2_")), - channel_throttle (k_param_channel_throttle, PSTR("RC3_")), - channel_rudder (k_param_channel_yaw, PSTR("RC4_")), - rc_5 (k_param_rc_5, PSTR("RC5_")), - rc_6 (k_param_rc_6, PSTR("RC6_")), - rc_7 (k_param_rc_7, PSTR("RC7_")), - rc_8 (k_param_rc_8, PSTR("RC8_")), - - // PID controller group key name initial P initial I initial D initial imax - //--------------------------------------------------------------------------------------------------------------------------------------- - pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), - pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), - pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), - pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSP2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), - pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), - pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), - pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), - - - junk(0) // XXX just so that we can add things without worrying about the trailing comma - { - } -}; - +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#include + +// Global parameter class. +// +class Parameters { +public: + // The version of the layout as described by the parameter enum. + // + // When changing the parameter enum in an incompatible fashion, this + // value should be incremented by one. + // + // The increment will prevent old parameters from being used incorrectly + // by newer code. + // + static const uint16_t k_format_version = 6; + + // + // Parameter identities. + // + // The enumeration defined here is used to ensure that every parameter + // or parameter group has a unique ID number. This number is used by + // AP_Var to store and locate parameters in EEPROM. + // + // Note that entries without a number are assigned the next number after + // the entry preceding them. When adding new entries, ensure that they + // don't overlap. + // + // Try to group related variables together, and assign them a set + // range in the enumeration. Place these groups in numerical order + // at the end of the enumeration. + // + // WARNING: Care should be taken when editing this enumeration as the + // AP_Var load/save code depends on the values here to identify + // variables saved in EEPROM. + // + // + enum { + // Layout version number, always key zero. + // + k_param_format_version = 0, + + + // Misc + // + + k_param_auto_trim, + k_param_switch_enable, + k_param_log_bitmask, + k_param_pitch_trim, + + // 110: Telemetry control + // + k_param_streamrates_port0 = 110, + k_param_streamrates_port3, + k_param_sysid_this_mav, + k_param_sysid_my_gcs, + + // 120: Fly-by-wire control + // + k_param_flybywire_airspeed_min = 120, + k_param_flybywire_airspeed_max, + + // + // 140: Sensor parameters + // + k_param_IMU_calibration = 140, + k_param_altitude_mix, + k_param_airspeed_ratio, + k_param_ground_temperature, + k_param_ground_pressure, + k_param_compass_enabled, + k_param_compass, + k_param_battery_monitoring, + k_param_pack_capacity, + + // + // 160: Navigation parameters + // + k_param_crosstrack_gain = 160, + k_param_crosstrack_entry_angle, + k_param_roll_limit, + k_param_pitch_limit_max, + k_param_pitch_limit_min, + k_param_airspeed_cruise, + k_param_RTL_altitude, + + // + // 180: Radio settings + // + k_param_channel_roll = 180, + k_param_channel_pitch, + k_param_channel_throttle, + k_param_channel_yaw, + k_param_rc_5, + k_param_rc_6, + k_param_rc_7, + k_param_rc_8, + k_param_rc_9, + k_param_rc_10, + + k_param_throttle_min, + k_param_throttle_max, + k_param_throttle_fs_enabled, + k_param_throttle_fs_value, + k_param_throttle_cruise, + k_param_flight_mode_channel, + k_param_flight_modes, + + k_param_short_fs_action, + k_param_long_fs_action, + k_param_gcs_heartbeat_fs_enabled, + + // + // 200: Feed-forward gains + // + k_param_kff_pitch_compensation = 200, + k_param_kff_rudder_mix, + k_param_kff_pitch_to_throttle, + k_param_kff_throttle_to_pitch, + + // + // 220: Waypoint data + // + k_param_waypoint_mode = 220, + k_param_waypoint_total, + k_param_waypoint_index, + k_param_waypoint_radius, + k_param_loiter_radius, + + // + // 240: PID Controllers + // + // Heading-to-roll PID: + // heading error from commnd to roll command deviation from trim + // (bank to turn strategy) + // + k_param_heading_to_roll_PID = 240, + + // Roll-to-servo PID: + // roll error from command to roll servo deviation from trim + // (tracks commanded bank angle) + // + k_param_roll_to_servo_PID, + + // + // Pitch control + // + // Pitch-to-servo PID: + // pitch error from command to pitch servo deviation from trim + // (front-side strategy) + // + k_param_pitch_to_servo_PID, + + // Airspeed-to-pitch PID: + // airspeed error from commnd to pitch servo deviation from trim + // (back-side strategy) + // + k_param_airspeed_to_pitch_PID, + + // + // Yaw control + // + // Yaw-to-servo PID: + // yaw rate error from commnd to yaw servo deviation from trim + // (stabilizes dutch roll) + // + k_param_yaw_to_servo_PID, + + // + // Throttle control + // + // Energy-to-throttle PID: + // total energy error from command to throttle servo deviation from trim + // (throttle back-side strategy alternative) + // + k_param_energy_to_throttle_PID, + + // Altitude-to-pitch PID: + // altitude error from commnd to pitch servo deviation from trim + // (throttle front-side strategy alternative) + // + k_param_altitude_to_pitch_PID, + + // 255: reserved + }; + + AP_Int16 format_version; + + // Telemetry control + // + AP_Int16 sysid_this_mav; + AP_Int16 sysid_my_gcs; + + // Feed-forward gains + // + AP_Float kff_pitch_compensation; + AP_Float kff_rudder_mix; + AP_Float kff_pitch_to_throttle; + AP_Float kff_throttle_to_pitch; + + // Crosstrack navigation + // + AP_Float crosstrack_gain; + AP_Int16 crosstrack_entry_angle; + + // Estimation + // + AP_Float altitude_mix; + AP_Float airspeed_ratio; + + // Waypoints + // + AP_Int8 waypoint_mode; + AP_Int8 waypoint_total; + AP_Int8 waypoint_index; + AP_Int8 waypoint_radius; + AP_Int8 loiter_radius; + + // Fly-by-wire + // + AP_Int8 flybywire_airspeed_min; + AP_Int8 flybywire_airspeed_max; + + // Throttle + // + AP_Int8 throttle_min; + AP_Int8 throttle_max; + AP_Int8 throttle_fs_enabled; + AP_Int16 throttle_fs_value; + AP_Int8 throttle_cruise; + + // Failsafe + AP_Int8 short_fs_action; + AP_Int8 long_fs_action; + AP_Int8 gcs_heartbeat_fs_enabled; + + // Flight modes + // + AP_Int8 flight_mode_channel; + AP_VarA flight_modes; + + // Navigational maneuvering limits + // + AP_Int16 roll_limit; + AP_Int16 pitch_limit_max; + AP_Int16 pitch_limit_min; + + // Misc + // + AP_Int8 auto_trim; + AP_Int8 switch_enable; + AP_Int16 log_bitmask; + AP_Int16 airspeed_cruise; + AP_Int16 pitch_trim; + AP_Int16 RTL_altitude; + AP_Int16 ground_temperature; + AP_Int32 ground_pressure; + AP_Int8 compass_enabled; + AP_Int16 angle_of_attack; + AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current + AP_Int16 pack_capacity; // Battery pack capacity less reserve + + // RC channels + RC_Channel channel_roll; + RC_Channel channel_pitch; + RC_Channel channel_throttle; + RC_Channel channel_rudder; + RC_Channel rc_5; + RC_Channel rc_6; + RC_Channel rc_7; + RC_Channel rc_8; + RC_Channel rc_camera_pitch; + RC_Channel rc_camera_roll; + + // PID controllers + // + PID pidNavRoll; + PID pidServoRoll; + PID pidServoPitch; + PID pidNavPitchAirspeed; + PID pidServoRudder; + PID pidTeThrottle; + PID pidNavPitchAltitude; + + uint8_t junk; + + // Note: keep initializers here in the same order as they are declared above. + Parameters() : + // variable default key name + //------------------------------------------------------------------------------------------------------- + format_version (k_format_version, k_param_format_version, NULL), + + sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), + sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), + + kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")), + kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")), + kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")), + kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")), + + crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), + crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), + + altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")), + airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")), + + /* XXX waypoint_mode missing here */ + waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), + waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), + loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), + + flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")), + flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")), + + throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), + throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), + throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), + throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), + throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), + + short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), + long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), + gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), + + flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")), + flight_modes (k_param_flight_modes, PSTR("FLTMODE")), + + roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")), + pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")), + pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")), + + auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")), + switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")), + log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")), + airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), + pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), + RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), + ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), + ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), + compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), + battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), + pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), + + // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! + + // RC channel group key name + //---------------------------------------------------------------------- + channel_roll (k_param_channel_roll, PSTR("RC1_")), + channel_pitch (k_param_channel_pitch, PSTR("RC2_")), + channel_throttle (k_param_channel_throttle, PSTR("RC3_")), + channel_rudder (k_param_channel_yaw, PSTR("RC4_")), + rc_5 (k_param_rc_5, PSTR("RC5_")), + rc_6 (k_param_rc_6, PSTR("RC6_")), + rc_7 (k_param_rc_7, PSTR("RC7_")), + rc_8 (k_param_rc_8, PSTR("RC8_")), + rc_camera_pitch (k_param_rc_9, NULL), + rc_camera_roll (k_param_rc_10, NULL), + + // PID controller group key name initial P initial I initial D initial imax + //--------------------------------------------------------------------------------------------------------------------------------------- + pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), + pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), + pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), + pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSPD2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), + pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), + pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), + pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), + + + junk(0) // XXX just so that we can add things without worrying about the trailing comma + { + } +}; + #endif // PARAMETERS_H diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde new file mode 100644 index 0000000000..733a81a129 --- /dev/null +++ b/ArduPlane/WP_activity.pde @@ -0,0 +1,49 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +void waypoint_check() +{ + if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want + if(wp_distance < 10){ // Get as close as it can for you + servo_pic(); + } // DO SOMETHIMNG + } + if(g.waypoint_index == 20){ // When here do whats underneath + servo_pic(); + } +} + +void picture_time_check() +{ + if (picture_time == 1){ + if (wp_distance < 10){ + servo_pic(); // or any camera activation command + } + } +} + +void egg_waypoint() +{ + float temp = (float)(current_loc.alt - home.alt) * .01; + float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; + + if(g.waypoint_index == 3){ + if(wp_distance < egg_dist){ + APM_RC.OutputCh(CH_RUDDER,1500 + (-45*10.31)); + } + }else{ + APM_RC.OutputCh(CH_RUDDER,1500 + (45*10.31)); + } +} + +void johann_check() // if you aren't Johann it doesn't really matter :D +{ + APM_RC.OutputCh(CH_7,1500 + (350)); + if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want + if(wp_distance < 10){ // Get as close as it can for you + servo_pic(); + } + } + if(g.waypoint_index == 20){ // When here do whats underneath + servo_pic(); + } +} diff --git a/ArduPlane/climb_rate.pde b/ArduPlane/climb_rate.pde index b7e6b81d74..ec1247a85e 100644 --- a/ArduPlane/climb_rate.pde +++ b/ArduPlane/climb_rate.pde @@ -1,12 +1,10 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - struct DataPoint { unsigned long x; long y; }; DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from -unsigned char hindex; // Index in history for the current data point +unsigned char index; // Index in history for the current data point unsigned long xoffset; unsigned char n; @@ -17,9 +15,12 @@ long yi; long xiyi; unsigned long xi2; -#if 0 // currently unused -static void add_altitude_data(unsigned long xl, long y) + +void add_altitude_data(unsigned long xl, long y) { + unsigned char i; + int dx; + //Reset the regression if our X variable overflowed if (xl < xoffset) n = 0; @@ -29,10 +30,10 @@ static void add_altitude_data(unsigned long xl, long y) n = 0; if (n == ALTITUDE_HISTORY_LENGTH) { - xi -= history[hindex].x; - yi -= history[hindex].y; - xiyi -= (long)history[hindex].x * history[hindex].y; - xi2 -= history[hindex].x * history[hindex].x; + xi -= history[index].x; + yi -= history[index].y; + xiyi -= (long)history[index].x * history[index].y; + xi2 -= history[index].x * history[index].x; } else { if (n == 0) { xoffset = xl; @@ -44,33 +45,31 @@ static void add_altitude_data(unsigned long xl, long y) n++; } - history[hindex].x = xl - xoffset; - history[hindex].y = y; + history[index].x = xl - xoffset; + history[index].y = y; - xi += history[hindex].x; - yi += history[hindex].y; - xiyi += (long)history[hindex].x * history[hindex].y; - xi2 += history[hindex].x * history[hindex].x; + xi += history[index].x; + yi += history[index].y; + xiyi += (long)history[index].x * history[index].y; + xi2 += history[index].x * history[index].x; - if (++hindex >= ALTITUDE_HISTORY_LENGTH) - hindex = 0; + if (++index >= ALTITUDE_HISTORY_LENGTH) + index = 0; } -#endif -#if 0 // unused -static void recalc_climb_rate() +void recalc_climb_rate() { float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2); climb_rate = (int)(slope*100); } -static void print_climb_debug_info() +void print_climb_debug_info() { unsigned char i, j; recalc_climb_rate(); //SendDebugln_P("Climb rate:"); for (i=0; i= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH; //SendDebug_P(" "); //SendDebug(j,DEC); @@ -91,4 +90,3 @@ static void print_climb_debug_info() //SendDebug((float)climb_rate/100,2); //SendDebugln_P(" m/s"); } -#endif diff --git a/ArduPlane/command description.txt b/ArduPlane/command description.txt index 2d3102c9d9..b2668a8b90 100644 --- a/ArduPlane/command description.txt +++ b/ArduPlane/command description.txt @@ -45,7 +45,7 @@ May Commands - these commands are optional to finish Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) - -0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT - alt (finish) rate (cm/sec) - +0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - - Note: rate must be > 10 cm/sec due to integer math MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 0ad04ee3da..5ccbf7e486 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -1,6 +1,6 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void init_commands() +void init_commands() { //read_EEPROM_waypoint_info(); g.waypoint_index.set_and_save(0); @@ -9,19 +9,15 @@ static void init_commands() next_command.id = CMD_BLANK; } -static void update_auto() +void update_auto() { - if (g.waypoint_index >= g.waypoint_total) { - handle_no_commands(); - if(g.waypoint_total == 0) { - next_WP.lat = home.lat + 1000; // so we don't have bad calcs - next_WP.lng = home.lng + 1000; // so we don't have bad calcs - } + if (g.waypoint_index == g.waypoint_total) { + do_RTL(); } } // this is only used by an air-start -static void reload_commands_airstart() +void reload_commands_airstart() { init_commands(); g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? @@ -30,7 +26,7 @@ static void reload_commands_airstart() // Getters // ------- -static struct Location get_wp_with_index(int i) +struct Location get_wp_with_index(int i) { struct Location temp; long mem; @@ -70,13 +66,12 @@ static struct Location get_wp_with_index(int i) // Setters // ------- -static void set_wp_with_index(struct Location temp, int i) +void set_wp_with_index(struct Location temp, int i) { i = constrain(i, 0, g.waypoint_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - - // Set altitude options bitmask - // XXX What is this trying to do? + + // Set altitude options bitmask if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){ temp.options = MASK_OPTIONS_RELATIVE_ALT; } else { @@ -101,26 +96,26 @@ static void set_wp_with_index(struct Location temp, int i) eeprom_write_dword((uint32_t *) mem, temp.lng); } -static void increment_WP_index() +void increment_WP_index() { - if (g.waypoint_index <= g.waypoint_total) { + if (g.waypoint_index < g.waypoint_total) { g.waypoint_index.set_and_save(g.waypoint_index + 1); - SendDebug_P("MSG - WP index is incremented to "); + SendDebug_P("MSG WP index is incremented to "); }else{ //SendDebug_P("MSG Failed to increment WP index of "); // This message is used excessively at the end of a mission } - //SendDebugln(g.waypoint_index,DEC); + SendDebugln(g.waypoint_index,DEC); } -static void decrement_WP_index() +void decrement_WP_index() { if (g.waypoint_index > 0) { g.waypoint_index.set_and_save(g.waypoint_index - 1); } } -static long read_alt_to_hold() +long read_alt_to_hold() { if(g.RTL_altitude < 0) return current_loc.alt; @@ -129,14 +124,30 @@ static long read_alt_to_hold() } +//******************************************************************************** +//This function sets the waypoint and modes for Return to Launch +//******************************************************************************** + +Location get_LOITER_home_wp() +{ + //so we know where we are navigating from + next_WP = current_loc; + + // read home position + struct Location temp = get_wp_with_index(0); + temp.id = MAV_CMD_NAV_LOITER_UNLIM; + temp.alt = read_alt_to_hold(); + return temp; +} + /* This function stores waypoint commands It looks to see what the next command type is and finds the last command. */ -static void set_next_WP(struct Location *wp) +void set_next_WP(struct Location *wp) { //gcs.send_text_P(SEVERITY_LOW,PSTR("load WP")); - SendDebug_P("MSG - wp_index: "); + SendDebug_P("MSG wp_index: "); SendDebugln(g.waypoint_index, DEC); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -162,44 +173,6 @@ static void set_next_WP(struct Location *wp) loiter_sum = 0; loiter_total = 0; - // this is used to offset the shrinking longitude as we go towards the poles - float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); - // this is handy for the groundstation - wp_totalDistance = get_distance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - target_bearing = get_bearing(¤t_loc, &next_WP); - nav_bearing = target_bearing; - - // to check if we have missed the WP - // ---------------------------- - old_target_bearing = target_bearing; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); - - gcs.print_current_waypoints(); -} - -static void set_guided_WP(void) -{ - SendDebug_P("MSG - set_guided_wp"); - - // copy the current location into the OldWP slot - // --------------------------------------- - prev_WP = current_loc; - - // Load the next_WP slot - // --------------------- - next_WP = guided_WP; - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - target_altitude = current_loc.alt; - offset_altitude = next_WP.alt - prev_WP.alt; - // this is used to offset the shrinking longitude as we go towards the poles float rads = (abs(next_WP.lat)/t7) * 0.0174532925; scaleLongDown = cos(rads); @@ -217,13 +190,15 @@ static void set_guided_WP(void) // set a new crosstrack bearing // ---------------------------- reset_crosstrack(); + + gcs.print_current_waypoints(); } // run this at setup on the ground // ------------------------------- void init_home() { - SendDebugln("MSG: init home"); + SendDebugln("MSG: init home"); // block until we get a good fix // ----------------------------- @@ -246,12 +221,6 @@ void init_home() // Save prev loc // ------------- next_WP = prev_WP = home; - - // Load home for a default guided_WP - // ------------- - guided_WP = home; - guided_WP.alt += g.RTL_altitude; - } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 7da8327970..1abbfe3693 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -3,7 +3,7 @@ /********************************************************************************/ // Command Event Handlers /********************************************************************************/ -static void +void handle_process_must() { // reset navigation integrators @@ -39,13 +39,13 @@ handle_process_must() case MAV_CMD_NAV_RETURN_TO_LAUNCH: do_RTL(); break; - + default: break; } } -static void +void handle_process_may() { switch(next_command.id){ @@ -82,7 +82,7 @@ handle_process_may() } } -static void handle_process_now() +void handle_process_now() { switch(next_command.id){ @@ -116,18 +116,27 @@ static void handle_process_now() } } -static void handle_no_commands() +void handle_no_commands() { - next_command = home; - next_command.alt = read_alt_to_hold(); - next_command.id = MAV_CMD_NAV_LOITER_UNLIM; + if (command_must_ID) + return; + + switch (control_mode){ + case LAND: + // don't get a new command + break; + + default: + set_mode(RTL); + break; + } } /********************************************************************************/ // Verify command Handlers /********************************************************************************/ -static bool verify_must() +bool verify_must() { switch(command_must_ID) { @@ -166,7 +175,7 @@ static bool verify_must() } } -static bool verify_may() +bool verify_may() { switch(command_may_ID) { case NO_COMMAND: @@ -186,18 +195,18 @@ static bool verify_may() default: gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); + return false; break; } - return false; } /********************************************************************************/ // Nav (Must) commands /********************************************************************************/ -static void do_RTL(void) +void do_RTL(void) { - control_mode = RTL; + control_mode = LOITER; crash_timer = 0; next_WP = home; @@ -213,7 +222,7 @@ static void do_RTL(void) Log_Write_Mode(control_mode); } -static void do_takeoff() +void do_takeoff() { set_next_WP(&next_command); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 @@ -227,28 +236,28 @@ static void do_takeoff() takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction } -static void do_nav_wp() +void do_nav_wp() { set_next_WP(&next_command); } -static void do_land() +void do_land() { set_next_WP(&next_command); } -static void do_loiter_unlimited() +void do_loiter_unlimited() { set_next_WP(&next_command); } -static void do_loiter_turns() +void do_loiter_turns() { set_next_WP(&next_command); loiter_total = next_command.p1 * 360; } -static void do_loiter_time() +void do_loiter_time() { set_next_WP(&next_command); loiter_time = millis(); @@ -258,7 +267,7 @@ static void do_loiter_time() /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ -static bool verify_takeoff() +bool verify_takeoff() { if (g_gps->ground_speed > 300){ if(hold_course == -1){ @@ -287,7 +296,7 @@ static bool verify_takeoff() } } -static bool verify_land() +bool verify_land() { // we don't verify landing - we never go to a new Must command after Land if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) @@ -314,7 +323,7 @@ static bool verify_land() return false; } -static bool verify_nav_wp() +bool verify_nav_wp() { hold_course = -1; update_crosstrack(); @@ -335,25 +344,25 @@ static bool verify_nav_wp() return false; } -static bool verify_loiter_unlim() +bool verify_loiter_unlim() { update_loiter(); calc_bearing_error(); return false; } -static bool verify_loiter_time() +bool verify_loiter_time() { update_loiter(); calc_bearing_error(); - if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds + if ((millis() - loiter_time) > (long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); return true; } return false; } -static bool verify_loiter_turns() +bool verify_loiter_turns() { update_loiter(); calc_bearing_error(); @@ -366,7 +375,7 @@ static bool verify_loiter_turns() return false; } -static bool verify_RTL() +bool verify_RTL() { if (wp_distance <= g.waypoint_radius) { gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home")); @@ -380,22 +389,22 @@ static bool verify_RTL() // Condition (May) commands /********************************************************************************/ -static void do_wait_delay() +void do_wait_delay() { condition_start = millis(); condition_value = next_command.lat * 1000; // convert to milliseconds } -static void do_change_alt() +void do_change_alt() { - condition_rate = next_command.lat; + condition_rate = next_command.p1; condition_value = next_command.alt; target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update next_WP.alt = condition_value; // For future nav calculations offset_altitude = 0; // For future nav calculations } -static void do_within_distance() +void do_within_distance() { condition_value = next_command.lat; } @@ -404,18 +413,23 @@ static void do_within_distance() // Verify Condition (May) commands /********************************************************************************/ -static bool verify_wait_delay() +bool verify_wait_delay() { - if ((unsigned)(millis() - condition_start) > condition_value){ + if ((millis() - condition_start) > condition_value){ condition_value = 0; return true; } return false; } -static bool verify_change_alt() +bool verify_change_alt() { - if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) { + //XXX this doesn't look right. How do you descend? + if(current_loc.alt >= condition_value) { + //Serial.printf_P(PSTR("alt, top:")); + //Serial.print(current_loc.alt, DEC); + //Serial.printf_P(PSTR("\t")); + //Serial.println(condition_value, DEC); condition_value = 0; return true; } @@ -423,7 +437,7 @@ static bool verify_change_alt() return false; } -static bool verify_within_distance() +bool verify_within_distance() { if (wp_distance < condition_value){ condition_value = 0; @@ -436,12 +450,12 @@ static bool verify_within_distance() // Do (Now) commands /********************************************************************************/ -static void do_loiter_at_location() +void do_loiter_at_location() { next_WP = current_loc; } -static void do_jump() +void do_jump() { struct Location temp; if(next_command.lat > 0) { @@ -453,22 +467,20 @@ static void do_jump() set_wp_with_index(temp, g.waypoint_index); g.waypoint_index.set_and_save(next_command.p1 - 1); - } else if (next_command.lat == -1) { - g.waypoint_index.set_and_save(next_command.p1 - 1); } } -static void do_change_speed() +void do_change_speed() { // Note: we have no implementation for commanded ground speed, only air speed and throttle if(next_command.alt > 0) g.airspeed_cruise.set_and_save(next_command.alt * 100); if(next_command.lat > 0) - g.throttle_cruise.set_and_save(next_command.lat); + g.throttle_cruise.set_and_save(next_command.lat * 100); } -static void do_set_home() +void do_set_home() { if(next_command.p1 == 1 && GPS_enabled) { init_home(); @@ -481,12 +493,12 @@ static void do_set_home() } } -static void do_set_servo() +void do_set_servo() { APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); } -static void do_set_relay() +void do_set_relay() { if (next_command.p1 == 1) { relay_on(); @@ -497,7 +509,7 @@ static void do_set_relay() } } -static void do_repeat_servo() +void do_repeat_servo() { event_id = next_command.p1 - 1; @@ -526,11 +538,11 @@ static void do_repeat_servo() } } -static void do_repeat_relay() +void do_repeat_relay() { event_id = RELAY_TOGGLE; event_timer = 0; event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_repeat = next_command.alt * 2; update_events(); -} +} diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index c0b2576e92..466ad2589a 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -2,15 +2,15 @@ // For changing active command mid-mission //---------------------------------------- -static void change_command(uint8_t cmd_index) +void change_command(uint8_t index) { - struct Location temp = get_wp_with_index(cmd_index); + struct Location temp = get_wp_with_index(index); if (temp.id > MAV_CMD_NAV_LAST ){ gcs.send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { command_must_index = NO_COMMAND; next_command.id = NO_COMMAND; - g.waypoint_index.set_and_save(cmd_index - 1); + g.waypoint_index.set_and_save(index - 1); load_next_command_from_EEPROM(); process_next_command(); } @@ -19,7 +19,7 @@ static void change_command(uint8_t cmd_index) // called by 10 Hz loop // -------------------- -static void update_commands(void) +void update_commands(void) { // This function loads commands into three buffers // when a new command is loaded, it is processed with process_XXX() @@ -34,7 +34,7 @@ static void update_commands(void) } // Other (eg GCS_Auto) modes may be implemented here } -static void verify_commands(void) +void verify_commands(void) { if(verify_must()){ command_must_index = NO_COMMAND; @@ -46,7 +46,7 @@ static void verify_commands(void) } } -static void load_next_command_from_EEPROM() +void load_next_command_from_EEPROM() { // fetch next command if the next command queue is empty // ----------------------------------------------------- @@ -64,7 +64,7 @@ static void load_next_command_from_EEPROM() } } -static void process_next_command() +void process_next_command() { // these are Navigation/Must commands // --------------------------------- @@ -115,7 +115,7 @@ static void process_next_command() /**************************************************/ // These functions implement the commands. /**************************************************/ -static void process_must() +void process_must() { gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -132,7 +132,7 @@ static void process_must() next_command.id = NO_COMMAND; } -static void process_may() +void process_may() { gcs.send_text_P(SEVERITY_LOW,PSTR("")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -145,7 +145,7 @@ static void process_may() next_command.id = NO_COMMAND; } -static void process_now() +void process_now() { handle_process_now(); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index bc1056e55f..003c1ea8e3 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -46,31 +46,13 @@ // AIRSPEED_SENSOR // AIRSPEED_RATIO // -#ifndef AIRSPEED_SENSOR -# define AIRSPEED_SENSOR DISABLED -#endif - +//#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 -////////////////////////////////////////////////////////////////////////////// -// Sonar -// - -#ifndef SONAR_ENABLED -# define SONAR_ENABLED DISABLED -#endif - -#ifndef SONAR_PIN -# define SONAR_PIN AN4 // AN5, AP_RANGEFINDER_PITOT_TUBE -#endif - -#ifndef SONAR_TYPE -# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL, -#endif - - ////////////////////////////////////////////////////////////////////////////// // HIL_PROTOCOL OPTIONAL // HIL_MODE OPTIONAL @@ -187,6 +169,9 @@ #ifndef MAG_ORIENTATION # define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #endif +#ifndef MAG_PROTOCOL +# define MAG_PROTOCOL MAG_PROTOCOL_5843 +#endif ////////////////////////////////////////////////////////////////////////////// @@ -199,7 +184,8 @@ ////////////////////////////////////////////////////////////////////////////// // Radio channel limits // -// Note that these are not called out in APM_Config.h.reference. +// Note that these are not called out in APM_Config.h.example as there +// is currently no configured behaviour for these channels. // #ifndef CH5_MIN # define CH5_MIN 1000 @@ -226,32 +212,7 @@ # define CH8_MAX 2000 #endif -////////////////////////////////////////////////////////////////////////////// -#ifndef RC_5_FUNCT -# define RC_5_FUNCT RC_5_FUNCT_NONE -#endif -#ifndef RC_6_FUNCT -# define RC_6_FUNCT RC_6_FUNCT_NONE -#endif -#ifndef RC_7_FUNCT -# define RC_7_FUNCT RC_7_FUNCT_NONE -#endif -#ifndef RC_8_FUNCT -# define RC_8_FUNCT RC_8_FUNCT_NONE -#endif -#ifndef FLAP_1_PERCENT -# define FLAP_1_PERCENT 0 -#endif -#ifndef FLAP_1_SPEED -# define FLAP_1_SPEED 255 -#endif -#ifndef FLAP_2_PERCENT -# define FLAP_2_PERCENT 0 -#endif -#ifndef FLAP_2_SPEED -# define FLAP_2_SPEED 255 -#endif ////////////////////////////////////////////////////////////////////////////// // FLIGHT_MODE // FLIGHT_MODE_CHANNEL @@ -295,14 +256,14 @@ #ifndef THROTTLE_FAILSAFE # define THROTTLE_FAILSAFE ENABLED #endif -#ifndef THROTTLE_FS_VALUE +#ifndef THROTTE_FS_VALUE # define THROTTLE_FS_VALUE 950 #endif #ifndef SHORT_FAILSAFE_ACTION -# define SHORT_FAILSAFE_ACTION 0 +# define SHORT_FAILSAFE_ACTION 2 #endif #ifndef LONG_FAILSAFE_ACTION -# define LONG_FAILSAFE_ACTION 0 +# define LONG_FAILSAFE_ACTION 2 #endif #ifndef GCS_HEARTBEAT_FAILSAFE # define GCS_HEARTBEAT_FAILSAFE DISABLED @@ -369,22 +330,6 @@ # define REVERSE_SWITCH ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// ENABLE ELEVON_MIXING -// -#ifndef ELEVON_MIXING -# define ELEVON_MIXING DISABLED -#endif -#ifndef ELEVON_REVERSE -# define ELEVON_REVERSE DISABLED -#endif -#ifndef ELEVON_CH1_REVERSE -# define ELEVON_CH1_REVERSE DISABLED -#endif -#ifndef ELEVON_CH2_REVERSE -# define ELEVON_CH2_REVERSE DISABLED -#endif - ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// @@ -611,12 +556,6 @@ ////////////////////////////////////////////////////////////////////////////// // Dataflash logging control // - -#ifndef LOGGING_ENABLED -# define LOGGING_ENABLED ENABLED -#endif - - #ifndef LOG_ATTITUDE_FAST # define LOG_ATTITUDE_FAST DISABLED #endif @@ -648,22 +587,6 @@ # define LOG_CUR DISABLED #endif -// calculate the default log_bitmask -#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) - -#define DEFAULT_LOG_BITMASK \ - LOGBIT(ATTITUDE_FAST) | \ - LOGBIT(ATTITUDE_MED) | \ - LOGBIT(GPS) | \ - LOGBIT(PM) | \ - LOGBIT(CTUN) | \ - LOGBIT(NTUN) | \ - LOGBIT(MODE) | \ - LOGBIT(RAW) | \ - LOGBIT(CMD) | \ - LOGBIT(CUR) - - #ifndef DEBUG_PORT # define DEBUG_PORT 0 #endif @@ -710,10 +633,6 @@ # define USE_CURRENT_ALT FALSE #endif -#ifndef INVERTED_FLIGHT_PWM -# define INVERTED_FLIGHT_PWM 1750 -#endif - ////////////////////////////////////////////////////////////////////////////// // Developer Items // @@ -724,17 +643,3 @@ #endif #define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE) -// use this to enable servos in HIL mode -#ifndef HIL_SERVOS -# define HIL_SERVOS DISABLED -#endif - -// use this to completely disable the CLI -#ifndef CLI_ENABLED -# define CLI_ENABLED ENABLED -#endif - -// delay to prevent Xbee bricking, in milliseconds -#ifndef MAVLINK_TELEMETRY_PORT_DELAY -# define MAVLINK_TELEMETRY_PORT_DELAY 2000 -#endif diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index 4c679f2e52..b760195eb9 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -1,11 +1,9 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -static void read_control_switch() +void read_control_switch() { byte switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition){ - set_mode(flight_modes[switchPosition]); + set_mode(g.flight_modes[switchPosition]); oldSwitchPosition = switchPosition; prev_WP = current_loc; @@ -14,15 +12,9 @@ static void read_control_switch() // ------------------------- reset_I(); } - - if (g.inverted_flight_ch != 0) { - // if the user has configured an inverted flight channel, then - // fly upside down when that channel goes above INVERTED_FLIGHT_PWM - inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); - } } -static byte readSwitch(void){ +byte readSwitch(void){ uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; @@ -32,7 +24,7 @@ static byte readSwitch(void){ return 0; } -static void reset_control_switch() +void reset_control_switch() { oldSwitchPosition = 0; read_control_switch(); @@ -40,23 +32,52 @@ static void reset_control_switch() SendDebugln(oldSwitchPosition , DEC); } -static void update_servo_switches() +void update_servo_switches() { - if (!g.switch_enable) { - // switches are disabled in EEPROM (see SWITCH_ENABLE option) - // this means the EEPROM control of all channel reversal is enabled - return; - } // up is reverse // up is elevon - g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode - if (g.mix_mode == 0) { - g.channel_roll.set_reverse((PINE & 128) ? true : false); - g.channel_pitch.set_reverse((PINE & 64) ? true : false); - g.channel_rudder.set_reverse((PINL & 64) ? true : false); + mix_mode = (PINL & 128) ? 1 : 0; + if (mix_mode == 0) { + reverse_roll = (PINE & 128) ? true : false; + reverse_pitch = (PINE & 64) ? true : false; + reverse_rudder = (PINL & 64) ? true : false; } else { - g.reverse_elevons = (PINE & 128) ? true : false; - g.reverse_ch1_elevon = (PINE & 64) ? true : false; - g.reverse_ch2_elevon = (PINL & 64) ? true : false; + reverse_elevons = (PINE & 128) ? -1 : 1; + reverse_ch1_elevon = (PINE & 64) ? -1 : 1; + reverse_ch2_elevon = (PINL & 64) ? -1 : 1; + } +} + + +bool trim_flag; +unsigned long trim_timer; + +// read at 10 hz +// set this to your trainer switch +void read_trim_switch() +{ + // switch is engaged + if (g.rc_7.control_in > 800){ + if(trim_flag == false){ + // called once + trim_timer = millis(); + } + trim_flag = true; + + }else{ // switch is disengaged + + if(trim_flag){ + // switch was just released + if((millis() - trim_timer) > 2000){ + + g.throttle_cruise.set_and_save(g.channel_throttle.control_in); + g.angle_of_attack.set_and_save(dcm.pitch_sensor); + g.airspeed_cruise.set_and_save(airspeed); + + } else { + + } + trim_flag = false; + } } } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b48deed2da..43ac0226d6 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -10,9 +10,11 @@ #define DEBUG 0 #define LOITER_RANGE 60 // for calculating power outside of loiter radius -#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. +#define ROLL_SERVO_MAX 4500 +#define PITCH_SERVO_MAX 4500 +#define RUDDER_SERVO_MAX 4500 -// failsafe +// failsafe // ---------------------- #define FAILSAFE_NONE 0 #define FAILSAFE_SHORT 1 @@ -30,6 +32,10 @@ #define T6 1000000 #define T7 10000000 +//MAGNETOMETER +#define MAG_PROTOCOL_5843 0 +#define MAG_PROTOCOL_5883L 1 + // GPS type codes - use the names, not the numbers #define GPS_PROTOCOL_NONE -1 #define GPS_PROTOCOL_NMEA 0 @@ -60,20 +66,6 @@ #define CH_RUDDER CH_4 #define CH_YAW CH_4 -#define RC_5_FUNCT_NONE 0 -#define RC_5_FUNCT_AILERON 1 -#define RC_5_FUNCT_FLAP_AUTO 2 -#define RC_5_FUNCT_FLAPERON 3 - -#define RC_6_FUNCT_NONE 0 -#define RC_6_FUNCT_AILERON 1 -#define RC_6_FUNCT_FLAP_AUTO 2 -#define RC_6_FUNCT_FLAPERON 3 - -#define RC_7_FUNCT_NONE 0 - -#define RC_8_FUNCT_NONE 0 - // HIL enumerations #define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_MAVLINK 2 @@ -98,15 +90,12 @@ #define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle #define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed -#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed - // Fly By Wire B and Fly By Wire C require airspeed sensor + // Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0 #define AUTO 10 #define RTL 11 #define LOITER 12 -//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM -//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM -#define GUIDED 15 -#define INITIALISING 16 // in startup routines +#define TAKEOFF 13 +#define LAND 14 // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library @@ -129,10 +118,6 @@ #define MAV_CMD_CONDITION_YAW 23 // GCS Message ID's -/// NOTE: to ensure we never block on sending MAVLink messages -/// please keep each MSG_ to a single MAVLink message. If need be -/// create new MSG_ IDs for additional messages on the same -/// stream #define MSG_ACKNOWLEDGE 0x00 #define MSG_HEARTBEAT 0x01 #define MSG_ATTITUDE 0x02 @@ -143,10 +128,9 @@ #define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed #define MSG_VERSION_REQUEST 0x08 #define MSG_VERSION 0x09 -#define MSG_EXTENDED_STATUS1 0x0a -#define MSG_EXTENDED_STATUS2 0x0b -#define MSG_CPU_LOAD 0x0c -#define MSG_NAV_CONTROLLER_OUTPUT 0x0d +#define MSG_EXTENDED_STATUS 0x0a +#define MSG_CPU_LOAD 0x0b +#define MSG_NAV_CONTROLLER_OUTPUT 0x0c #define MSG_COMMAND_REQUEST 0x20 #define MSG_COMMAND_UPLOAD 0x21 @@ -169,11 +153,9 @@ #define MSG_RADIO_OUT 0x53 #define MSG_RADIO_IN 0x54 -#define MSG_RAW_IMU1 0x60 -#define MSG_RAW_IMU2 0x61 -#define MSG_RAW_IMU3 0x62 -#define MSG_GPS_STATUS 0x63 -#define MSG_GPS_RAW 0x64 +#define MSG_RAW_IMU 0x60 +#define MSG_GPS_STATUS 0x61 +#define MSG_GPS_RAW 0x62 #define MSG_SERVO_OUT 0x70 @@ -191,7 +173,6 @@ #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 #define MSG_LOCAL_LOCATION 0xb4 -#define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1 #define SEVERITY_MEDIUM 2 @@ -224,6 +205,7 @@ #define MASK_LOG_RAW (1<<7) #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CUR (1<<9) +#define MASK_LOG_SET_DEFAULTS (1<<15) // Waypoint Modes // ---------------- @@ -264,11 +246,7 @@ // sonar -#define MAX_SONAR_XL 0 -#define MAX_SONAR_LV 1 #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters -#define AN4 4 -#define AN5 5 // Hardware Parameters #define SLIDE_SWITCH_PIN 40 @@ -289,6 +267,3 @@ #define ONBOARD_PARAM_NAME_LENGTH 15 #define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe - -// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1) -#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1) diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index acd60e9cfc..e0445f06d1 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void failsafe_short_on_event() +void failsafe_short_on_event() { // This is how to handle a short loss of control signal failsafe. failsafe = FAILSAFE_SHORT; @@ -18,7 +18,7 @@ static void failsafe_short_on_event() case AUTO: case LOITER: - if(g.short_fs_action == 1) { + if(g.short_fs_action != 1) { set_mode(RTL); } break; @@ -32,11 +32,10 @@ static void failsafe_short_on_event() SendDebugln(control_mode, DEC); } -static void failsafe_long_on_event() +void failsafe_long_on_event() { - // This is how to handle a long loss of control signal failsafe. + // This is how to handle a short loss of control signal failsafe. SendDebug_P("Failsafe - Long event on"); - APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC switch(control_mode) { case MANUAL: @@ -49,7 +48,7 @@ static void failsafe_long_on_event() case AUTO: case LOITER: case CIRCLE: - if(g.long_fs_action == 1) { + if(g.long_fs_action != 1) { set_mode(RTL); } break; @@ -60,7 +59,7 @@ static void failsafe_long_on_event() } } -static void failsafe_short_off_event() +void failsafe_short_off_event() { // We're back in radio contact SendDebug_P("Failsafe - Short event off"); @@ -75,16 +74,15 @@ static void failsafe_short_off_event() reset_I(); } -#if BATTERY_EVENT == ENABLED -static void low_battery_event(void) +void low_battery_event(void) { gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); set_mode(RTL); g.throttle_cruise = THROTTLE_CRUISE; } -#endif -static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY + +void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY { if(event_repeat == 0 || (millis() - event_timer) < event_delay) return; @@ -110,17 +108,17 @@ static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_ } } -static void relay_on() +void relay_on() { PORTL |= B00000100; } -static void relay_off() +void relay_off() { PORTL &= ~B00000100; } -static void relay_toggle() +void relay_toggle() { PORTL ^= B00000100; } diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 487ffe4a3b..6090510d23 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -3,7 +3,7 @@ //**************************************************************** // Function that will calculate the desired direction to fly and distance //**************************************************************** -static void navigate() +void navigate() { // do not navigate with corrupt data // --------------------------------- @@ -63,7 +63,7 @@ void calc_distance_error() } #endif -static void calc_airspeed_errors() +void calc_airspeed_errors() { // XXX excess casting here if(control_mode>=AUTO && airspeed_nudge > 0) { @@ -75,7 +75,7 @@ static void calc_airspeed_errors() } } -static void calc_bearing_error() +void calc_bearing_error() { if(takeoff_complete == true || g.compass_enabled == true) { bearing_error = nav_bearing - dcm.yaw_sensor; @@ -89,7 +89,7 @@ static void calc_bearing_error() bearing_error = wrap_180(bearing_error); } -static void calc_altitude_error() +void calc_altitude_error() { if(control_mode == AUTO && offset_altitude != 0) { // limit climb rates @@ -127,39 +127,38 @@ static void calc_altitude_error() altitude_error = target_altitude - current_loc.alt; } -static long wrap_360(long error) +long wrap_360(long error) { if (error > 36000) error -= 36000; if (error < 0) error += 36000; return error; } -static long wrap_180(long error) +long wrap_180(long error) { if (error > 18000) error -= 36000; if (error < -18000) error += 36000; return error; } -static void update_loiter() +void update_loiter() { float power; if(wp_distance <= g.loiter_radius){ power = float(wp_distance) / float(g.loiter_radius); - power = constrain(power, 0.5, 1); 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.5, 1); //power = constrain(power, 0, 1); + 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{ @@ -167,27 +166,35 @@ static void update_loiter() } update_crosstrack(); -*/ nav_bearing = wrap_360(nav_bearing); } -static void update_crosstrack(void) +void update_crosstrack(void) { // Crosstrack Error // ---------------- if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line + crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing); } } -static void reset_crosstrack() +void reset_crosstrack() { crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } -static long get_distance(struct Location *loc1, struct Location *loc2) +long get_altitude_above_home(void) // TO DO - this function has been deprecated - check if any useage vs removal +{ + // This is the altitude above the home location + // The GPS gives us altitude at Sea Level + // if you slope soar, you should see a negative number sometimes + // ------------------------------------------------------------- + return current_loc.alt - home.alt; +} + +long get_distance(struct Location *loc1, struct Location *loc2) { if(loc1->lat == 0 || loc1->lng == 0) return -1; @@ -198,7 +205,12 @@ static long get_distance(struct Location *loc1, struct Location *loc2) return sqrt(sq(dlat) + sq(dlong)) * .01113195; } -static long get_bearing(struct Location *loc1, struct Location *loc2) +long get_alt_distance(struct Location *loc1, struct Location *loc2) +{ + return abs(loc1->alt - loc2->alt); +} + +long get_bearing(struct Location *loc1, struct Location *loc2) { long off_x = loc2->lng - loc1->lng; long off_y = (loc2->lat - loc1->lat) * scaleLongUp; @@ -206,3 +218,14 @@ static long get_bearing(struct Location *loc1, struct Location *loc2) if (bearing < 0) bearing += 36000; return bearing; } + +Vector3f get_location_vector(struct Location *uav, struct Location *target) +{ + Vector3 v; + v.x = (target->lng-uav->lng) * cos((uav->lat+target->lat)/2)*.01113195; + v.y = (target->lat-uav->lat)*.01113195; + v.z = (uav->alt-target->alt); + return v; +} + + diff --git a/ArduPlane/planner.pde b/ArduPlane/planner.pde index 3154f18084..9497110381 100644 --- a/ArduPlane/planner.pde +++ b/ArduPlane/planner.pde @@ -8,19 +8,18 @@ static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details -static const struct Menu::command planner_menu_commands[] PROGMEM = { +const struct Menu::command planner_menu_commands[] PROGMEM = { {"gcs", planner_gcs}, }; // A Macro to create the Menu MENU(planner_menu, "planner", planner_menu_commands); -static int8_t +int8_t planner_mode(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); planner_menu.run(); - return 0; } static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv) @@ -45,6 +44,4 @@ planner_gcs(uint8_t argc, const Menu::arg *argv) loopcount++; } } - return 0; -} - +} diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 748b7a9228..0e0028ab90 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -2,18 +2,21 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling -static void init_rc_in() +void init_rc_in() { // set rc reversing update_servo_switches(); + g.channel_roll.set_reverse(reverse_roll); + g.channel_pitch.set_reverse(reverse_pitch); + g.channel_rudder.set_reverse(reverse_rudder); // set rc channel ranges - g.channel_roll.set_angle(SERVO_MAX); - g.channel_pitch.set_angle(SERVO_MAX); - g.channel_rudder.set_angle(SERVO_MAX); + g.channel_roll.set_angle(ROLL_SERVO_MAX); + g.channel_pitch.set_angle(PITCH_SERVO_MAX); + g.channel_rudder.set_angle(RUDDER_SERVO_MAX); g.channel_throttle.set_range(0, 100); // set rc dead zones @@ -23,27 +26,13 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.set_angle(SERVO_MAX); - } else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) { - g.rc_5.set_range(0,100); - } else { - g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - if (g.rc_6_funct == RC_6_FUNCT_AILERON) { - g.rc_6.set_angle(SERVO_MAX); - } else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) { - g.rc_6.set_range(0,100); - } else { - g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here + g.rc_5.set_range(0,1000); + g.rc_6.set_range(0,1000); + g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); } -static void init_rc_out() +void init_rc_out() { APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); @@ -68,17 +57,17 @@ static void init_rc_out() APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); } -static void read_radio() +void read_radio() { ch1_temp = APM_RC.InputCh(CH_ROLL); ch2_temp = APM_RC.InputCh(CH_PITCH); - if(g.mix_mode == 0){ + if(mix_mode == 0){ g.channel_roll.set_pwm(ch1_temp); g.channel_pitch.set_pwm(ch2_temp); }else{ - g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); - g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); + g.channel_roll.set_pwm(reverse_elevons * (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) - reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500); + g.channel_pitch.set_pwm((reverse_ch2_elevon * int(ch2_temp - elevon2_trim) + reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500); } g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); @@ -98,7 +87,7 @@ static void read_radio() g.channel_throttle.servo_out = g.channel_throttle.control_in; if (g.channel_throttle.servo_out > 50) { - if(g.airspeed_enabled == true) { + if(airspeed_enabled == true) { airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); } else { throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); @@ -117,7 +106,7 @@ static void read_radio() */ } -static void control_failsafe(uint16_t pwm) +void control_failsafe(uint16_t pwm) { if(g.throttle_fs_enabled == 0) return; @@ -132,7 +121,7 @@ static void control_failsafe(uint16_t pwm) //Check for failsafe and debounce funky reads } else if (g.throttle_fs_enabled) { - if (pwm < (unsigned)g.throttle_fs_value){ + if (pwm < g.throttle_fs_value){ // we detect a failsafe from radio // throttle has dropped below the mark failsafeCounter++; @@ -164,18 +153,15 @@ static void control_failsafe(uint16_t pwm) } } -static void trim_control_surfaces() +void trim_control_surfaces() { read_radio(); // Store control surface trim values // --------------------------------- - if(g.mix_mode == 0){ + if(mix_mode == 0){ g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel - }else{ elevon1_trim = ch1_temp; elevon2_trim = ch2_temp; @@ -191,11 +177,9 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); } -static void trim_radio() +void trim_radio() { for (int y = 0; y < 30; y++) { read_radio(); @@ -203,13 +187,11 @@ static void trim_radio() // Store the trim values // --------------------- - if(g.mix_mode == 0){ + if(mix_mode == 0){ g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -225,7 +207,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); } diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 3f7001d1ad..d74ffcb060 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -5,17 +5,21 @@ void ReadSCP1000(void) {} -static void init_barometer(void) +void init_barometer(void) { - int flashcount = 0; + int flashcount; long ground_pressure = 0; int ground_temperature; + #if HIL_MODE == HIL_MODE_SENSORS + hil.update(); // look for inbound hil packets for initialization + #endif + while(ground_pressure == 0){ barometer.Read(); // Get initial data from absolute pressure sensor ground_pressure = barometer.Press; ground_temperature = barometer.Temp; - mavlink_delay(20); + delay(20); //Serial.printf("barometer.Press %ld\n", barometer.Press); } @@ -29,7 +33,7 @@ static void init_barometer(void) ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l; ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; - mavlink_delay(20); + delay(20); if(flashcount == 5) { digitalWrite(C_LED_PIN, LOW); digitalWrite(A_LED_PIN, HIGH); @@ -49,11 +53,11 @@ static void init_barometer(void) g.ground_temperature.set_and_save(ground_temperature / 10.0f); abs_pressure = ground_pressure; - Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); +Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); + SendDebugln_P("barometer calibration complete."); } -static long read_barometer(void) +long read_barometer(void) { float x, scaling, temp; @@ -69,38 +73,30 @@ static long read_barometer(void) } // in M/S * 100 -static void read_airspeed(void) +void read_airspeed(void) { #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed - if (g.airspeed_offset == 0) { - // runtime enabling of airspeed, we need to do instant - // calibration before we can use it. This isn't as - // accurate as the 50 point average in zero_airspeed(), - // but it is better than using it uncalibrated - airspeed_raw = (float)adc.Ch(AIRSPEED_CH); - g.airspeed_offset.set_and_save(airspeed_raw); - } airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75); - airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0); + airspeed_pressure = max(((int)airspeed_raw - airspeed_offset), 0); airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100; #endif calc_airspeed_errors(); } -static void zero_airspeed(void) +void zero_airspeed(void) { airspeed_raw = (float)adc.Ch(AIRSPEED_CH); - for(int c = 0; c < 10; c++){ + for(int c = 0; c < 50; c++){ delay(20); airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10); } - g.airspeed_offset.set_and_save(airspeed_raw); + airspeed_offset = airspeed_raw; } #endif // HIL_MODE != HIL_MODE_ATTITUDE -static void read_battery(void) +void read_battery(void) { battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; @@ -118,7 +114,7 @@ static void read_battery(void) current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; } - #if BATTERY_EVENT == ENABLED + #if BATTERY_EVENT == 1 if(battery_voltage < LOW_VOLTAGE) low_battery_event(); if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); #endif diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 2841e8b787..7d35d953c6 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -1,7 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if CLI_ENABLED == ENABLED - // Functions called from the setup menu static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); @@ -13,7 +11,7 @@ static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); // Command/function table for the setup menu -static const struct Menu::command setup_menu_commands[] PROGMEM = { +const struct Menu::command setup_menu_commands[] PROGMEM = { // command function called // ======= =============== {"reset", setup_factory}, @@ -30,7 +28,7 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = { MENU(setup_menu, "setup", setup_menu_commands); // Called from the top-level menu to run the setup menu. -static int8_t +int8_t setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance @@ -43,7 +41,6 @@ setup_mode(uint8_t argc, const Menu::arg *argv) // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); - return 0; } // Print the current configuration. @@ -51,6 +48,7 @@ setup_mode(uint8_t argc, const Menu::arg *argv) static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { + uint8_t i; // clear the area print_blanks(8); @@ -75,6 +73,8 @@ setup_show(uint8_t argc, const Menu::arg *argv) static int8_t setup_factory(uint8_t argc, const Menu::arg *argv) { + + uint8_t i; int c; Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); @@ -185,7 +185,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { - byte switchPosition, mode = 0; + byte switchPosition, oldSwitchPosition, mode; Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); @@ -201,10 +201,10 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) if (oldSwitchPosition != switchPosition){ // force position 5 to MANUAL if (switchPosition > 4) { - flight_modes[switchPosition] = MANUAL; + g.flight_modes[switchPosition] = MANUAL; } // update our current mode - mode = flight_modes[switchPosition]; + mode = g.flight_modes[switchPosition]; // update the user print_switch(switchPosition, mode); @@ -228,11 +228,13 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode != FLY_BY_WIRE_B && mode != AUTO && mode != RTL && - mode != LOITER) + mode != LOITER && + mode != TAKEOFF && + mode != LAND) { if (mode < MANUAL) - mode = LOITER; - else if (mode >LOITER) + mode = LAND; + else if (mode >LAND) mode = MANUAL; else mode += radioInputSwitch; @@ -243,7 +245,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode = MANUAL; // save new mode - flight_modes[switchPosition] = mode; + g.flight_modes[switchPosition] = mode; // print new mode print_switch(switchPosition, mode); @@ -252,8 +254,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) // escape hatch if(Serial.available() > 0){ // save changes - for (mode=0; mode<6; mode++) - flight_modes[mode].save(); + g.flight_modes.save(); report_flight_modes(); print_done(); return (0); @@ -266,13 +267,13 @@ setup_declination(uint8_t argc, const Menu::arg *argv) { compass.set_declination(radians(argv[1].f)); report_compass(); - return 0; } static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) { + uint8_t i; int c; Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ")); @@ -291,13 +292,9 @@ static int8_t setup_compass(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("on"))) { - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - g.compass_enabled = false; - } else { - g.compass_enabled = true; - } + g.compass_enabled = true; + bool junkbool = compass.init(); + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.compass_enabled = false; @@ -326,11 +323,50 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv) return 0; } +/***************************************************************************/ +// CLI defaults +/***************************************************************************/ + +void +default_flight_modes() +{ + g.flight_modes[0] = FLIGHT_MODE_1; + g.flight_modes[1] = FLIGHT_MODE_2; + g.flight_modes[2] = FLIGHT_MODE_3; + g.flight_modes[3] = FLIGHT_MODE_4; + g.flight_modes[4] = FLIGHT_MODE_5; + g.flight_modes[5] = FLIGHT_MODE_6; + g.flight_modes.save(); +} + +void +default_log_bitmask() +{ + + // convenience macro for testing LOG_* and setting LOGBIT_* + #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) + + g.log_bitmask = + LOGBIT(ATTITUDE_FAST) | + LOGBIT(ATTITUDE_MED) | + LOGBIT(GPS) | + LOGBIT(PM) | + LOGBIT(CTUN) | + LOGBIT(NTUN) | + LOGBIT(MODE) | + LOGBIT(RAW) | + LOGBIT(CMD) | + LOGBIT(CUR); + #undef LOGBIT + + g.log_bitmask.save(); +} + /***************************************************************************/ // CLI reports /***************************************************************************/ -static void report_batt_monitor() +void report_batt_monitor() { //print_blanks(2); Serial.printf_P(PSTR("Batt Mointor\n")); @@ -342,7 +378,7 @@ static void report_batt_monitor() if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); print_blanks(2); } -static void report_radio() +void report_radio() { //print_blanks(2); Serial.printf_P(PSTR("Radio\n")); @@ -352,7 +388,7 @@ static void report_radio() print_blanks(2); } -static void report_gains() +void report_gains() { //print_blanks(2); Serial.printf_P(PSTR("Gains\n")); @@ -370,7 +406,7 @@ static void report_gains() Serial.printf_P(PSTR("nav roll:\n")); print_PID(&g.pidNavRoll); - Serial.printf_P(PSTR("nav pitch airspeed:\n")); + Serial.printf_P(PSTR("nav pitch airpseed:\n")); print_PID(&g.pidNavPitchAirspeed); Serial.printf_P(PSTR("energry throttle:\n")); @@ -382,7 +418,7 @@ static void report_gains() print_blanks(2); } -static void report_xtrack() +void report_xtrack() { //print_blanks(2); Serial.printf_P(PSTR("Crosstrack\n")); @@ -395,7 +431,7 @@ static void report_xtrack() print_blanks(2); } -static void report_throttle() +void report_throttle() { //print_blanks(2); Serial.printf_P(PSTR("Throttle\n")); @@ -414,7 +450,7 @@ static void report_throttle() print_blanks(2); } -static void report_imu() +void report_imu() { //print_blanks(2); Serial.printf_P(PSTR("IMU\n")); @@ -425,32 +461,16 @@ static void report_imu() print_blanks(2); } -static void report_compass() +void report_compass() { //print_blanks(2); - Serial.printf_P(PSTR("Compass: ")); - - switch (compass.product_id) { - case AP_COMPASS_TYPE_HMC5883L: - Serial.println_P(PSTR("HMC5883L")); - break; - case AP_COMPASS_TYPE_HMC5843: - Serial.println_P(PSTR("HMC5843")); - break; - case AP_COMPASS_TYPE_HIL: - Serial.println_P(PSTR("HIL")); - break; - default: - Serial.println_P(PSTR("??")); - break; - } - + Serial.printf_P(PSTR("Compass\n")); print_divider(); print_enabled(g.compass_enabled); // mag declination - Serial.printf_P(PSTR("Mag Declination: %4.4f\n"), + Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), degrees(compass.get_declination())); Vector3f offsets = compass.get_offsets(); @@ -463,14 +483,14 @@ static void report_compass() print_blanks(2); } -static void report_flight_modes() +void report_flight_modes() { //print_blanks(2); Serial.printf_P(PSTR("Flight modes\n")); print_divider(); for(int i = 0; i < 6; i++ ){ - print_switch(i, flight_modes[i]); + print_switch(i, g.flight_modes[i]); } print_blanks(2); } @@ -479,7 +499,7 @@ static void report_flight_modes() // CLI utilities /***************************************************************************/ -static void +void print_PID(PID * pid) { Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), @@ -489,7 +509,7 @@ print_PID(PID * pid) (long)pid->imax()); } -static void +void print_radio_values() { Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); @@ -503,20 +523,20 @@ print_radio_values() } -static void +void print_switch(byte p, byte m) { Serial.printf_P(PSTR("Pos %d: "),p); Serial.println(flight_mode_strings[m]); } -static void +void print_done() { Serial.printf_P(PSTR("\nSaved Settings\n\n")); } -static void +void print_blanks(int num) { while(num > 0){ @@ -525,7 +545,7 @@ print_blanks(int num) } } -static void +void print_divider(void) { for (int i = 0; i < 40; i++) { @@ -534,7 +554,7 @@ print_divider(void) Serial.println(""); } -static int8_t +int8_t radio_input_switch(void) { static int8_t bouncer = 0; @@ -561,9 +581,9 @@ radio_input_switch(void) } -static void zero_eeprom(void) +void zero_eeprom(void) { - byte b = 0; + byte b; Serial.printf_P(PSTR("\nErasing EEPROM\n")); for (int i = 0; i < EEPROM_MAX_ADDR; i++) { eeprom_write_byte((uint8_t *) i, b); @@ -571,7 +591,7 @@ static void zero_eeprom(void) Serial.printf_P(PSTR("done\n")); } -static void print_enabled(bool b) +void print_enabled(bool b) { if(b) Serial.printf_P(PSTR("en")); @@ -580,7 +600,7 @@ static void print_enabled(bool b) Serial.printf_P(PSTR("abled\n")); } -static void +void print_accel_offsets(void) { Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), @@ -589,7 +609,7 @@ print_accel_offsets(void) (float)imu.az()); } -static void +void print_gyro_offsets(void) { Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), @@ -599,4 +619,3 @@ print_gyro_offsets(void) } -#endif // CLI_ENABLED diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index cbe7ddadd2..b447ef3b57 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -6,13 +6,11 @@ The init_ardupilot function processes everything we need for an in - air restart *****************************************************************************/ -#if CLI_ENABLED == ENABLED - // Functions called from the top-level menu -static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp -static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde +extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde +extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp +extern int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -30,7 +28,7 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) } // Command/function table for the top-level menu. -static const struct Menu::command main_menu_commands[] PROGMEM = { +const struct Menu::command main_menu_commands[] PROGMEM = { // command function called // ======= =============== {"logs", process_logs}, @@ -41,11 +39,9 @@ static const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "APM trunk", main_menu_commands); +MENU(main_menu, "ArduPilotMega", main_menu_commands); -#endif // CLI_ENABLED - -static void init_ardupilot() +void init_ardupilot() { // Console serial port // @@ -77,6 +73,16 @@ static void init_ardupilot() Serial1.begin(38400, 128, 16); #endif + // Telemetry port. + // + // Not used if telemetry is going to the console. + // + // XXX for unidirectional protocols, we could (should) minimize + // the receive buffer, and the transmit buffer could also be + // shrunk for protocols that don't send large messages. + // + Serial3.begin(SERIAL3_BAUD, 128, 128); + Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %lu\n"), freeRAM()); @@ -86,10 +92,7 @@ static void init_ardupilot() // if (!g.format_version.load()) { - Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n")); - delay(100); // wait for serial msg to flush - - AP_Var::erase_all(); + Serial.println("\n\nEEPROM blank - all parameters are reset to default values.\n"); // save the current format version g.format_version.set_and_save(Parameters::k_format_version); @@ -99,7 +102,6 @@ static void init_ardupilot() Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" "\n\nForcing complete parameter reset..."), g.format_version.get(), Parameters::k_format_version); - delay(100); // wait for serial msg to flush // erase all parameters AP_Var::erase_all(); @@ -114,48 +116,45 @@ static void init_ardupilot() AP_Var::load_all(); Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); - Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"), - AP_Var::get_memory_use(), (unsigned)g.num_resets); + Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); } - // keep a record of how many resets have happened. This can be - // used to detect in-flight resets - g.num_resets.set_and_save(g.num_resets+1); - - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + if (!g.flight_modes.load()) { + default_flight_modes(); + } + if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) { + default_log_bitmask(); + } mavlink_system.sysid = g.sysid_this_mav; +#if CAMERA == ENABLED + init_camera(); +#endif #if HIL_MODE != HIL_MODE_ATTITUDE adc.Init(); // APM ADC library initialization barometer.Init(); // APM Abs Pressure sensor initialization + // Autodetect airspeed sensor + if ((adc.Ch(AIRSPEED_CH) > 2000)&&(adc.Ch(AIRSPEED_CH) < 2900)) + { + airspeed_enabled = false; + } + else + { + airspeed_enabled = true; + } + if (g.compass_enabled==true) { - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - g.compass_enabled = false; - } else { - dcm.set_compass(&compass); - compass.get_offsets(); // load offsets to account for airframe magnetic interference - } + dcm.set_compass(&compass); + bool junkbool = compass.init(); + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference + //compass.set_declination(ToRad(get(PARAM_DECLINATION))); // TODO fix this to have a UI // set local difference between magnetic north and true north } - /* - Init is depricated - Jason - if(g.sonar_enabled){ - sonar.init(SONAR_PIN, &adc); - Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC); - } - */ +#else + airspeed_enabled = true; #endif DataFlash.Init(); // DataFlash log initialization @@ -163,7 +162,6 @@ static void init_ardupilot() // Do GPS init g_gps = &g_gps_driver; g_gps->init(); // GPS Initialization - g_gps->callback = mavlink_delay; // init the GCS #if GCS_PORT == 3 @@ -210,7 +208,6 @@ static void init_ardupilot() // the system in an odd state, we don't let the user exit the top // menu; they must reset in order to fly. // -#if CLI_ENABLED == ENABLED if (digitalRead(SLIDE_SWITCH_PIN) == 0) { digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED Serial.printf_P(PSTR("\n" @@ -224,7 +221,6 @@ static void init_ardupilot() main_menu.run(); } } -#endif // CLI_ENABLED if(g.log_bitmask != 0){ @@ -272,8 +268,6 @@ static void init_ardupilot() Log_Write_Startup(TYPE_GROUNDSTART_MSG); } - set_mode(MANUAL); - // set the correct flight mode // --------------------------- reset_control_switch(); @@ -282,10 +276,8 @@ static void init_ardupilot() //******************************************************************************** //This function does all the calibrations, etc. that we need during a ground start //******************************************************************************** -static void startup_ground(void) +void startup_ground(void) { - set_mode(INITIALISING); - gcs.send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); #if(GROUND_START_DELAY > 0) @@ -314,7 +306,7 @@ static void startup_ground(void) trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. #if HIL_MODE != HIL_MODE_ATTITUDE -if (g.airspeed_enabled == true) +if (airspeed_enabled == true) { // initialize airspeed sensor // -------------------------- @@ -357,7 +349,7 @@ else gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } -static void set_mode(byte mode) +void set_mode(byte mode) { if(control_mode == mode){ // don't switch modes if we are already in the correct mode. @@ -371,11 +363,18 @@ static void set_mode(byte mode) switch(control_mode) { - case INITIALISING: case MANUAL: + break; + case CIRCLE: + break; + case STABILIZE: + break; + case FLY_BY_WIRE_A: + break; + case FLY_BY_WIRE_B: break; @@ -391,8 +390,10 @@ static void set_mode(byte mode) do_loiter_at_location(); break; - case GUIDED: - set_guided_WP(); + case TAKEOFF: + break; + + case LAND: break; default: @@ -407,7 +408,7 @@ static void set_mode(byte mode) Log_Write_Mode(control_mode); } -static void check_long_failsafe() +void check_long_failsafe() { // only act on changes // ------------------- @@ -432,7 +433,7 @@ static void check_long_failsafe() } } -static void check_short_failsafe() +void check_short_failsafe() { // only act on changes // ------------------- @@ -441,7 +442,7 @@ static void check_short_failsafe() failsafe_short_on_event(); } } - + if(failsafe == FAILSAFE_SHORT){ if(!ch3_failsafe) { failsafe_short_off_event(); @@ -450,19 +451,20 @@ static void check_short_failsafe() } -static void startup_IMU_ground(void) +void startup_IMU_ground(void) { #if HIL_MODE != HIL_MODE_ATTITUDE - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); - mavlink_delay(500); + uint16_t store = 0; + SendDebugln(" Warming up ADC..."); + delay(500); // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! // ----------------------- demo_servos(2); - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); - mavlink_delay(1000); + SendDebugln(" Beginning IMU calibration; do not move plane"); + delay(1000); - imu.init(IMU::COLD_START, mavlink_delay); + imu.init(IMU::COLD_START); dcm.set_centripetal(1); // read Baro pressure at ground @@ -477,7 +479,7 @@ static void startup_IMU_ground(void) } -static void update_GPS_light(void) +void update_GPS_light(void) { // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- @@ -505,16 +507,15 @@ static void update_GPS_light(void) } -static void resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; +void resetPerfData(void) { + mainLoop_count = 0; + G_Dt_max = 0; dcm.gyro_sat_count = 0; imu.adc_constraints = 0; dcm.renorm_sqrt_count = 0; dcm.renorm_blowup_count = 0; - gps_fix_count = 0; - pmTest1 = 0; - perf_mon_timer = millis(); + gps_fix_count = 0; + perf_mon_timer = millis(); } @@ -525,7 +526,7 @@ static void resetPerfData(void) { * be larger than HP or you'll be in big trouble! The smaller the gap, the more * careful you need to be. Julian Gall 6 - Feb - 2009. */ -static unsigned long freeRAM() { +unsigned long freeRAM() { uint8_t * heapptr, * stackptr; stackptr = (uint8_t *)malloc(4); // use stackptr temporarily heapptr = stackptr; // save value of heap pointer @@ -534,20 +535,3 @@ static unsigned long freeRAM() { return stackptr - heapptr; } - -/* - map from a 8 bit EEPROM baud rate to a real baud rate - */ -static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) -{ - switch (rate) { - case 9: return 9600; - case 19: return 19200; - case 38: return 38400; - case 57: return 57600; - case 111: return 111100; - case 115: return 115200; - } - Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); - return default_baud; -} diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 926e95ca1a..9200012f40 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -1,6 +1,4 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CLI_ENABLED == ENABLED +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. @@ -28,7 +26,7 @@ static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details -static const struct Menu::command test_menu_commands[] PROGMEM = { +const struct Menu::command test_menu_commands[] PROGMEM = { {"pwm", test_radio_pwm}, {"radio", test_radio}, {"failsafe", test_failsafe}, @@ -66,15 +64,14 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { // A Macro to create the Menu MENU(test_menu, "test", test_menu_commands); -static int8_t +int8_t test_mode(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("Test Mode\n\n")); test_menu.run(); - return 0; } -static void print_hit_enter() +void print_hit_enter() { Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); } @@ -107,7 +104,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) // ---------------------------------------------------------- read_radio(); - Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.channel_roll.radio_in, g.channel_pitch.radio_in, g.channel_throttle.radio_in, @@ -150,7 +147,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) // write out the servo PWM values // ------------------------------ - set_servos(); + set_servos_4(); Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.channel_roll.control_in, @@ -258,7 +255,7 @@ if (g.battery_monitoring == 4) { // write out the servo PWM values // ------------------------------ - set_servos(); + set_servos_4(); if(Serial.available() > 0){ return (0); @@ -318,11 +315,11 @@ test_wp(uint8_t argc, const Menu::arg *argv) return (0); } -static void -test_wp_print(struct Location *cmd, byte wp_index) +void +test_wp_print(struct Location *cmd, byte index) { Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), - (int)wp_index, + (int)index, (int)cmd->id, (int)cmd->options, (int)cmd->p1, @@ -379,24 +376,20 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - if (!g.switch_enable) { - Serial.println_P(PSTR("dip switches disabled, using EEPROM")); - } - while(1){ delay(100); update_servo_switches(); - if (g.mix_mode == 0) { + if (mix_mode == 0) { Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), - (int)g.channel_roll.get_reverse(), - (int)g.channel_pitch.get_reverse(), - (int)g.channel_rudder.get_reverse()); + (int)reverse_roll, + (int)reverse_pitch, + (int)reverse_rudder); } else { Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), - (int)g.reverse_elevons, - (int)g.reverse_ch1_elevon, - (int)g.reverse_ch2_elevon); + (int)reverse_elevons, + (int)reverse_ch1_elevon, + (int)reverse_ch2_elevon); } if(Serial.available() > 0){ return (0); @@ -532,29 +525,13 @@ test_gyro(uint8_t argc, const Menu::arg *argv) static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { - if (!g.compass_enabled) { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); - } - - compass.set_orientation(MAG_ORIENTATION); - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - return 0; - } - dcm.set_compass(&compass); - report_compass(); - - // we need the DCM initialised for this test - imu.init(IMU::COLD_START); - int counter = 0; + if(g.compass_enabled) { //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); - print_hit_enter(); + print_hit_enter(); - while(1) { + while(1){ delay(20); if (millis() - fast_loopTimer > 19) { delta_ms_fast_loop = millis() - fast_loopTimer; @@ -565,38 +542,40 @@ test_mag(uint8_t argc, const Menu::arg *argv) // --- dcm.update_DCM(G_Dt); - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - medium_loopCounter = 0; - } + if(g.compass_enabled) { + medium_loopCounter++; + if(medium_loopCounter == 5){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + medium_loopCounter = 0; + } + } counter++; if (counter>20) { - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z, - maggy.x, - maggy.y, - maggy.z); - counter=0; - } - } - if (Serial.available() > 0) { - break; - } - } + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z, + maggy.x, + maggy.y, + maggy.z); + counter=0; +} - // save offsets. This allows you to get sane offset values using - // the CLI before you go flying. - Serial.println_P(PSTR("saving offsets")); - compass.save_offsets(); - return (0); + } + if(Serial.available() > 0){ + return (0); + } + } + } else { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } } #endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS @@ -609,11 +588,14 @@ test_mag(uint8_t argc, const Menu::arg *argv) static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv) { - unsigned airspeed_ch = adc.Ch(AIRSPEED_CH); // Serial.println(adc.Ch(AIRSPEED_CH)); - Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch); + if ((adc.Ch(AIRSPEED_CH) > 2000) && (adc.Ch(AIRSPEED_CH) < 2900)){ + airspeed_enabled = false; + }else{ + airspeed_enabled = true; + } - if (g.airspeed_enabled == false){ + if (airspeed_enabled == false){ Serial.printf_P(PSTR("airspeed: ")); print_enabled(false); return (0); @@ -632,8 +614,11 @@ test_airspeed(uint8_t argc, const Menu::arg *argv) if(Serial.available() > 0){ return (0); } + if(Serial.available() > 0){ + return (0); } } + } } @@ -684,5 +669,3 @@ test_rawgps(uint8_t argc, const Menu::arg *argv) } } #endif // HIL_MODE == HIL_MODE_DISABLED - -#endif // CLI_ENABLED From b9fad1262ac5b1c0df7ecf08280bc3e71558fa9d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:49:11 +1000 Subject: [PATCH 02/74] move Frame into ArduCopter --- .../Frame}/PD-PCB_BOTTOM-SOLDER.pdf | Bin {Frame => ArduCopter/Frame}/PD-PCB_BOTTOM_LAYER.pdf | Bin {Frame => ArduCopter/Frame}/PD-PCB_TOP_LAYER.pdf | Bin {Frame => ArduCopter/Frame}/PD-PCB_TOP_SOLDER.pdf | Bin {Frame => ArduCopter/Frame}/README | 0 .../Frame}/TradHeli/Trex450SonarMount.skp | Bin .../Frame}/TradHeli/apmTrex450MountV9.ai | 0 .../Frame}/TradHeli/apmTrex450MountV9.crv | Bin .../Frame}/TradHeli/apmTrex450MountV9.dxf | 0 .../Frame}/TradHeli/apmTrex450MountV9.eps | 0 {Frame => ArduCopter/Frame}/TradHeli/gpsMountV2.skp | Bin 11 files changed, 0 insertions(+), 0 deletions(-) rename {Frame => ArduCopter/Frame}/PD-PCB_BOTTOM-SOLDER.pdf (100%) rename {Frame => ArduCopter/Frame}/PD-PCB_BOTTOM_LAYER.pdf (100%) rename {Frame => ArduCopter/Frame}/PD-PCB_TOP_LAYER.pdf (100%) rename {Frame => ArduCopter/Frame}/PD-PCB_TOP_SOLDER.pdf (100%) rename {Frame => ArduCopter/Frame}/README (100%) rename {Frame => ArduCopter/Frame}/TradHeli/Trex450SonarMount.skp (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.ai (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.crv (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.dxf (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.eps (100%) rename {Frame => ArduCopter/Frame}/TradHeli/gpsMountV2.skp (100%) diff --git a/Frame/PD-PCB_BOTTOM-SOLDER.pdf b/ArduCopter/Frame/PD-PCB_BOTTOM-SOLDER.pdf similarity index 100% rename from Frame/PD-PCB_BOTTOM-SOLDER.pdf rename to ArduCopter/Frame/PD-PCB_BOTTOM-SOLDER.pdf diff --git a/Frame/PD-PCB_BOTTOM_LAYER.pdf b/ArduCopter/Frame/PD-PCB_BOTTOM_LAYER.pdf similarity index 100% rename from Frame/PD-PCB_BOTTOM_LAYER.pdf rename to ArduCopter/Frame/PD-PCB_BOTTOM_LAYER.pdf diff --git a/Frame/PD-PCB_TOP_LAYER.pdf b/ArduCopter/Frame/PD-PCB_TOP_LAYER.pdf similarity index 100% rename from Frame/PD-PCB_TOP_LAYER.pdf rename to ArduCopter/Frame/PD-PCB_TOP_LAYER.pdf diff --git a/Frame/PD-PCB_TOP_SOLDER.pdf b/ArduCopter/Frame/PD-PCB_TOP_SOLDER.pdf similarity index 100% rename from Frame/PD-PCB_TOP_SOLDER.pdf rename to ArduCopter/Frame/PD-PCB_TOP_SOLDER.pdf diff --git a/Frame/README b/ArduCopter/Frame/README similarity index 100% rename from Frame/README rename to ArduCopter/Frame/README diff --git a/Frame/TradHeli/Trex450SonarMount.skp b/ArduCopter/Frame/TradHeli/Trex450SonarMount.skp similarity index 100% rename from Frame/TradHeli/Trex450SonarMount.skp rename to ArduCopter/Frame/TradHeli/Trex450SonarMount.skp diff --git a/Frame/TradHeli/apmTrex450MountV9.ai b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.ai similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.ai rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.ai diff --git a/Frame/TradHeli/apmTrex450MountV9.crv b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.crv similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.crv rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.crv diff --git a/Frame/TradHeli/apmTrex450MountV9.dxf b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.dxf similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.dxf rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.dxf diff --git a/Frame/TradHeli/apmTrex450MountV9.eps b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.eps similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.eps rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.eps diff --git a/Frame/TradHeli/gpsMountV2.skp b/ArduCopter/Frame/TradHeli/gpsMountV2.skp similarity index 100% rename from Frame/TradHeli/gpsMountV2.skp rename to ArduCopter/Frame/TradHeli/gpsMountV2.skp From ec398505a6409959a220d7b76d5d1c025a5f6136 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:51:21 +1000 Subject: [PATCH 03/74] rename main pde file for arduino requirements --- ArduCopter/{ArduCopterMega.pde => ArduCopter.pde} | 0 ArduPlane/{ArduPilotMega.pde => ArduPlane.pde} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename ArduCopter/{ArduCopterMega.pde => ArduCopter.pde} (100%) rename ArduPlane/{ArduPilotMega.pde => ArduPlane.pde} (100%) diff --git a/ArduCopter/ArduCopterMega.pde b/ArduCopter/ArduCopter.pde similarity index 100% rename from ArduCopter/ArduCopterMega.pde rename to ArduCopter/ArduCopter.pde diff --git a/ArduPlane/ArduPilotMega.pde b/ArduPlane/ArduPlane.pde similarity index 100% rename from ArduPlane/ArduPilotMega.pde rename to ArduPlane/ArduPlane.pde From f40c85a6012524fdee73d9ae6dd87972531886ca Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Fri, 9 Sep 2011 16:02:22 +0200 Subject: [PATCH 04/74] This is the real HEAD of the APM_Camera branch. Seams that lots of changes got lost in the SVN to GIT port --- ArduPlane/Cam_move.pde | 210 ------------- ArduPlane/Cam_trigger.pde | 41 --- libraries/AP_Camera/AP_Camera.cpp | 116 ++++++++ libraries/AP_Camera/{Camera.h => AP_Camera.h} | 34 +-- libraries/AP_Camera/Camera.cpp | 277 ------------------ 5 files changed, 126 insertions(+), 552 deletions(-) delete mode 100644 ArduPlane/Cam_move.pde delete mode 100644 ArduPlane/Cam_trigger.pde create mode 100644 libraries/AP_Camera/AP_Camera.cpp rename libraries/AP_Camera/{Camera.h => AP_Camera.h} (63%) delete mode 100644 libraries/AP_Camera/Camera.cpp diff --git a/ArduPlane/Cam_move.pde b/ArduPlane/Cam_move.pde deleted file mode 100644 index bc8ad0eac7..0000000000 --- a/ArduPlane/Cam_move.pde +++ /dev/null @@ -1,210 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CAMERA == ENABLED - -void init_camera() -{ - g.rc_camera_pitch.set_angle(4500); // throw of servo? - g.rc_camera_pitch.radio_min = 1000; // lowest radio input - g.rc_camera_pitch.radio_trim = 1500; // middle radio input - g.rc_camera_pitch.radio_max = 2000; // highest radio input - - g.rc_camera_roll.set_angle(4500); - g.rc_camera_roll.radio_min = 1000; - g.rc_camera_roll.radio_trim = 1500; - g.rc_camera_roll.radio_max = 2000; - - - //use test target for now - camera_target = home; - -} - -void camera() -{ - //decide what happens to camera depending on camera mode - switch(camera_mode) - { - case 0: - //do nothing, i.e lock camera in place - break; - case 1: - //stabilize - target_vector.x=0; //east to west gives +tive value (i.e. longitude) - target_vector.y=0; //south to north gives +tive value (i.e. latitude) - target_vector.z=100; //downwards is +tive - camera_move(); - break; - case 2: - //track target - if(g_gps->fix) - { - target_vector=get_location_vector(¤t_loc,&camera_target); - camera_move(); - } - break; - } -} - -void camera_move() -{ - Matrix3f m = dcm.get_dcm_transposed(); - Vector3 targ = m*target_vector; //to do: find out notion of x y convention - - switch(gimbal_mode) - { - case 0: // pitch & roll - cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch - cam_roll = degrees(atan2(targ.y, targ.z)); //roll - // check_limits(pitch); - // check_limits(roll); - // camera_out(); - break; - - case 1: // pitch and yaw - cam_tilt = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; - cam_pan = 9000 + atan2(-targ.y, targ.x) * 5729.57795; - if (cam_pan < 0) cam_pan += 36000; - // check_limits(pitch); - // check_limits(yaw); - // camera_out(); - break; - - /* case 2: // pitch, roll & yaw - not started - float cam_ritch = 100; - float cam_yoll = 100; - float cam_paw = 100; - break; */ - } -} - -/* void check_limits(axis,variable) // Use servo definitions to calculate for all servo throws - TO DO -{ - // find limits of servo range in deg - track_pan_right = PAN_CENTER + (PAN_RANGE/2); - track_pan_left = track_pan_right + (360 - PAN_RANGE); - if (track_pan_left > 360){ - track_pan_left = track_pan_left - 360; - } - // check track_bearing is "safe" - not outside pan servo limits - // if the bearing lies in the servo dead zone change bearing to closet edge - if (track_bearing < track_pan_left && track_bearing > track_pan_right){ - track_oor_l = abs(track_bearing - track_pan_left); - track_oor_r = abs(track_bearing - track_pan_right); - if (track_oor_r > track_oor_l){ - track_bearing = track_pan_right; - } - if (track_oor_l > track_oor_r){ - track_bearing = track_pan_left; - } - } - // center bearing to cam_servo center - track_pan_deg = track_bearing - PAN_CENTER; - // make negative is left rotation - if (track_pan_deg > 180){ - track_pan_deg = (180 - (track_pan_deg - 180)) * -1; - } - -} */ - -void camera_out() -{ - switch(gimbal_mode) - { - case 0: // pitch & roll - g.rc_camera_pitch.servo_out = cam_pitch; - g.rc_camera_pitch.calc_pwm(); - g.rc_camera_roll.servo_out = cam_roll; - g.rc_camera_roll.calc_pwm(); - break; - - case 1: // pitch and yaw - g.rc_camera_pitch.servo_out = cam_tilt; - g.rc_camera_pitch.calc_pwm(); - g.rc_camera_roll.servo_out = cam_pan; // borrowing roll servo output for pan/yaw - g.rc_camera_roll.calc_pwm(); - break; - - /*case 2: // pitch, roll & yaw - g.rc_camera_pitch.servo_out = cam_ritch; - g.rc_camera_pitch.calc_pwm(); - - g.rc_camera_roll.servo_out = cam_yoll; - g.rc_camera_roll.calc_pwm(); - - g.rc_camera_yaw.servo_out = cam_paw; // camera_yaw doesn't exist it should unless we use another channel - g.rc_camera_yaw.calc_pwm(); - break; */ - } -#if defined PITCH_SERVO - APM_RC.OutputCh(PITCH_SERVO, g.rc_camera_pitch.radio_out); -#endif -#if defined ROLL_SERVO - APM_RC.OutputCh(ROLL_SERVO, g.rc_camera_roll.radio_out); -#endif -/*#if defined YAW_SERVO - APM_RC.OutputCh(YAW_SERVO, g.rc_camera_yaw.radio_out); -#endif */ - -#if CAM_DEBUG == ENABLED - //for debugging purposes - Serial.println(); - Serial.print("current_loc: lat: "); - Serial.print(current_loc.lat); - Serial.print(", lng: "); - Serial.print(current_loc.lng); - Serial.print(", alt: "); - Serial.print(current_loc.alt); - Serial.println(); - Serial.print("target_loc: lat: "); - Serial.print(camera_target.lat); - Serial.print(", lng: "); - Serial.print(camera_target.lng); - Serial.print(", alt: "); - Serial.print(camera_target.alt); - Serial.print(", distance: "); - Serial.print(get_distance(¤t_loc,&camera_target)); - Serial.print(", bearing: "); - Serial.print(get_bearing(¤t_loc,&camera_target)); - Serial.println(); - Serial.print("dcm_angles: roll: "); - Serial.print(degrees(dcm.roll)); - Serial.print(", pitch: "); - Serial.print(degrees(dcm.pitch)); - Serial.print(", yaw: "); - Serial.print(degrees(dcm.yaw)); - Serial.println(); - Serial.print("target_vector: x: "); - Serial.print(target_vector.x,2); - Serial.print(", y: "); - Serial.print(target_vector.y,2); - Serial.print(", z: "); - Serial.print(target_vector.z,2); - Serial.println(); - Serial.print("rotated_target_vector: x: "); - Serial.print(targ.x,2); - Serial.print(", y: "); - Serial.print(targ.y,2); - Serial.print(", z: "); - Serial.print(targ.z,2); - Serial.println(); - Serial.print("gimbal type 0: roll: "); - Serial.print(roll); - Serial.print(", pitch: "); - Serial.print(pitch); - Serial.println(); - /* Serial.print("gimbal type 1: pitch: "); - Serial.print(pan); - Serial.print(", roll: "); - Serial.print(tilt); - Serial.println(); */ - /* Serial.print("gimbal type 2: pitch: "); - Serial.print(ritch); - Serial.print(", roll: "); - Serial.print(yoll); - Serial.print(", yaw: "); - Serial.print(paw); - Serial.println(); */ -#endif -} -#endif diff --git a/ArduPlane/Cam_trigger.pde b/ArduPlane/Cam_trigger.pde deleted file mode 100644 index 7b03a90c8b..0000000000 --- a/ArduPlane/Cam_trigger.pde +++ /dev/null @@ -1,41 +0,0 @@ -void servo_pic() // Servo operated camera -{ - APM_RC.OutputCh(CAM_SERVO,1500 + (333)); // Camera click, not enough - add more, wring way - put a minus before bracket number (-300) - delay(250); // delay - APM_RC.OutputCh(CAM_SERVO,1500); // Return servo to mid position -} - -void relay_picture() // basic relay activation -{ - relay_on(); - delay(250); // 0.25 seconds delay - relay_off(); -} - -void throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic = 10){ - servo_pic(); // triggering method - thr_pic = 0; - g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method - g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void NPN_trigger() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // To Do: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - delay(50); - digitalWrite(camtrig, LOW); -} diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp new file mode 100644 index 0000000000..df32a4adac --- /dev/null +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -0,0 +1,116 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#include + +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function +extern long wp_distance; +extern "C" { +void relay_on(); +void relay_off(); +} + +void +AP_Camera::servo_pic() // Servo operated camera +{ + if (g_rc_function[RC_Channel_aux::k_cam_trigger]) + { + g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max; + keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles + } +} + +void +AP_Camera::relay_pic() // basic relay activation +{ + relay_on(); + keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles +} + +void +AP_Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. +{ +// TODO find a way to do this without using the global parameter g +// g.channel_throttle.radio_out = g.throttle_min; + if (thr_pic == 10){ + servo_pic(); // triggering method + thr_pic = 0; +// g.channel_throttle.radio_out = g.throttle_cruise; + } + thr_pic++; +} + +void +AP_Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. +{ +// TODO find a way to do this without using the global parameter g +// g.channel_throttle.radio_out = g.throttle_min; + if (wp_distance < 3){ + servo_pic(); // triggering method +// g.channel_throttle.radio_out = g.throttle_cruise; + } +} + +void +AP_Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. +{ + // TODO: Assign pin spare pin for output + digitalWrite(camtrig, HIGH); + keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles +} + +// single entry point to take pictures +void +AP_Camera::trigger_pic() +{ + switch (trigger_type) + { + case 0: + servo_pic(); // Servo operated camera + break; + case 1: + relay_pic(); // basic relay activation + break; + case 2: + throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. + break; + case 3: + distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. + break; + case 4: + NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. + break; + } +} + +// de-activate the trigger after some delay, but without using a delay() function +void +AP_Camera::trigger_pic_cleanup() +{ + if (keep_cam_trigg_active_cycles) + { + keep_cam_trigg_active_cycles --; + } + else + { + switch (trigger_type) + { + case 0: + case 2: + case 3: + if (g_rc_function[RC_Channel_aux::k_cam_trigger]) + { + g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; + } + break; + case 1: + // TODO for some strange reason the function call bellow gives a linker error + //relay_off(); + PORTL &= ~B00000100; // hardcoded version of relay_off(). Replace with a proper function call later. + break; + case 4: + digitalWrite(camtrig, LOW); + break; + } + } +} diff --git a/libraries/AP_Camera/Camera.h b/libraries/AP_Camera/AP_Camera.h similarity index 63% rename from libraries/AP_Camera/Camera.h rename to libraries/AP_Camera/AP_Camera.h index 88dc792748..e1c520131a 100644 --- a/libraries/AP_Camera/Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -1,16 +1,16 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -/// @file Camera.h +/// @file AP_Camera.h /// @brief Photo or video camera manager, with EEPROM-backed storage of constants. -#ifndef CAMERA_H -#define CAMERA_H +#ifndef AP_CAMERA_H +#define AP_CAMERA_H #include /// @class Camera /// @brief Object managing a Photo or video camera -class Camera{ +class AP_Camera{ protected: AP_Var_group _group; // must be before all vars to keep ctor init order correct @@ -20,45 +20,31 @@ public: /// @param key EEPROM storage key for the camera configuration parameters. /// @param name Optional name for the group. /// - Camera(AP_Var::Key key, const prog_char_t *name) : + AP_Camera(AP_Var::Key key, const prog_char_t *name) : _group(key, name), - mode (&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name - trigger_type(&_group, 1, 0, name ? PSTR("TRIGGER_MODE") : 0), + trigger_type(&_group, 0, 0, name ? PSTR("TRIGG_TYPE") : 0), picture_time (0), // waypoint trigger variable thr_pic (0), // timer variable for throttle_pic camtrig (83), // PK6 chosen as it not near anything so safer for soldering -// camera_target (home), // use test target for now - gimbal_type (1), - keep_cam_trigg_active_cycles (0) + keep_cam_trigg_active_cycles (0), + wp_distance_min (10) {} - // move the camera depending on the camera mode - void move(); - // single entry point to take pictures void trigger_pic(); // de-activate the trigger after some delay, but without using a delay() function void trigger_pic_cleanup(); - // call this from time to time to make sure the correct gimbal type gets choosen - void update_camera_gimbal_type(); - - // set camera orientation target - void set_target(struct Location target); - int picture_time; // waypoint trigger variable + long wp_distance_min; // take picture if distance to WP is smaller than this private: uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active - struct Location camera_target; // point of interest for the camera to track -// struct Location GPS_mark; // GPS POI for position based triggering int thr_pic; // timer variable for throttle_pic int camtrig; // PK6 chosen as it not near anything so safer for soldering - AP_Int8 mode; // 0=do nothing, 1=stabilize, 2=track target, 3=manual, 4=simple stabilize test AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor - uint8_t gimbal_type; // 0=pitch & roll (tilt & roll), 1=yaw & pitch(pan & tilt), 2=pitch, roll & yaw (to be added) void servo_pic(); // Servo operated camera void relay_pic(); // basic relay activation @@ -68,4 +54,4 @@ private: }; -#endif /* CAMERA_H */ +#endif /* AP_CAMERA_H */ diff --git a/libraries/AP_Camera/Camera.cpp b/libraries/AP_Camera/Camera.cpp deleted file mode 100644 index 2c9bb928fd..0000000000 --- a/libraries/AP_Camera/Camera.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include "Camera.h" -#include "../RC_Channel/RC_Channel.h" - -void -Camera::move() -{ - Vector3 target_vector(0,0,1); // x, y, z to target before rotating to planes axis, values are in meters - - //decide what happens to camera depending on camera mode - switch(mode) - { - case 0: - //do nothing, i.e lock camera in place - return; - break; - case 1: - //stabilize - target_vector.x=0; //east to west gives +tive value (i.e. longitude) - target_vector.y=0; //south to north gives +tive value (i.e. latitude) - target_vector.z=100; //downwards is +tive - break; - case 2: - //track target - if(g_gps->fix) - { - target_vector=get_location_vector(¤t_loc,&camera_target); - } - break; - case 3: // radio manual control - case 4: // test (level the camera and point north) - break; // see code 25 lines bellow - } - - Matrix3f m = dcm.get_dcm_transposed(); - Vector3 targ = m*target_vector; //to do: find out notion of x y convention - switch(gimbal_type) - { - case 0: // pitch & roll (tilt & roll) - cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch - cam_roll = degrees(atan2(targ.y, targ.z)); //roll - break; - - case 1: // yaw & pitch (pan & tilt) - cam_pitch = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; - cam_yaw = 9000 + atan2(-targ.y, targ.x) * 5729.57795; - break; - -/* case 2: // pitch, roll & yaw - not started - cam_ritch = 0; - cam_yoll = 0; - cam_paw = 0; - break; */ - - } - - //some camera modes overwrite the gimbal_type calculations - switch(mode) - { - case 3: // radio manual control - if (rc_function[CAM_PITCH]) - cam_pitch = map(rc_function[CAM_PITCH]->radio_in, - rc_function[CAM_PITCH]->radio_min, - rc_function[CAM_PITCH]->radio_max, - rc_function[CAM_PITCH]->angle_min, - rc_function[CAM_PITCH]->radio_max); - if (rc_function[CAM_ROLL]) - cam_roll = map(rc_function[CAM_ROLL]->radio_in, - rc_function[CAM_ROLL]->radio_min, - rc_function[CAM_ROLL]->radio_max, - rc_function[CAM_ROLL]->angle_min, - rc_function[CAM_ROLL]->radio_max); - if (rc_function[CAM_YAW]) - cam_yaw = map(rc_function[CAM_YAW]->radio_in, - rc_function[CAM_YAW]->radio_min, - rc_function[CAM_YAW]->radio_max, - rc_function[CAM_YAW]->angle_min, - rc_function[CAM_YAW]->radio_max); - break; - case 4: // test (level the camera and point north) - cam_pitch = -dcm.pitch_sensor; - cam_yaw = dcm.yaw_sensor; // do not invert because the servo is mounted upside-down on my system - // TODO: the "trunk" code can invert using parameters, but this branch still can't - cam_roll = -dcm.roll_sensor; - break; - } - - #if CAM_DEBUG == ENABLED - //for debugging purposes - Serial.println(); - Serial.print("current_loc: lat: "); - Serial.print(current_loc.lat); - Serial.print(", lng: "); - Serial.print(current_loc.lng); - Serial.print(", alt: "); - Serial.print(current_loc.alt); - Serial.println(); - Serial.print("target_loc: lat: "); - Serial.print(camera_target.lat); - Serial.print(", lng: "); - Serial.print(camera_target.lng); - Serial.print(", alt: "); - Serial.print(camera_target.alt); - Serial.print(", distance: "); - Serial.print(get_distance(¤t_loc,&camera_target)); - Serial.print(", bearing: "); - Serial.print(get_bearing(¤t_loc,&camera_target)); - Serial.println(); - Serial.print("dcm_angles: roll: "); - Serial.print(degrees(dcm.roll)); - Serial.print(", pitch: "); - Serial.print(degrees(dcm.pitch)); - Serial.print(", yaw: "); - Serial.print(degrees(dcm.yaw)); - Serial.println(); - Serial.print("target_vector: x: "); - Serial.print(target_vector.x,2); - Serial.print(", y: "); - Serial.print(target_vector.y,2); - Serial.print(", z: "); - Serial.print(target_vector.z,2); - Serial.println(); - Serial.print("rotated_target_vector: x: "); - Serial.print(targ.x,2); - Serial.print(", y: "); - Serial.print(targ.y,2); - Serial.print(", z: "); - Serial.print(targ.z,2); - Serial.println(); - Serial.print("gimbal type 0: roll: "); - Serial.print(roll); - Serial.print(", pitch: "); - Serial.print(pitch); - Serial.println(); - /* Serial.print("gimbal type 1: pitch: "); - Serial.print(pan); - Serial.print(", roll: "); - Serial.print(tilt); - Serial.println(); */ - /* Serial.print("gimbal type 2: pitch: "); - Serial.print(ritch); - Serial.print(", roll: "); - Serial.print(yoll); - Serial.print(", yaw: "); - Serial.print(paw); - Serial.println(); */ -#endif -} - - -void -Camera::set_target(struct Location target) -{ - camera_target = target; -} - - -void -Camera::update_camera_gimbal_type() -{ - - // Auto detect the camera gimbal type depending on the functions assigned to the servos - if ((rc_function[CAM_YAW] == NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL)) - { - gimbal_type = 0; - } - if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] == NULL)) - { - gimbal_type = 1; - } - if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL)) - { - gimbal_type = 2; - } -} - -void -Camera::servo_pic() // Servo operated camera -{ - if (rc_function[CAM_TRIGGER]) - { - cam_trigger = rc_function[CAM_TRIGGER]->radio_max; - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles - } -} - -void -Camera::relay_pic() // basic relay activation -{ - relay_on(); - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles -} - -void -Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic == 10){ - servo_pic(); // triggering method - thr_pic = 0; - g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void -Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method - g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void -Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // To Do: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles -} - -// single entry point to take pictures -void -Camera::trigger_pic() -{ - switch (trigger_type) - { - case 0: - servo_pic(); // Servo operated camera - break; - case 1: - relay_pic(); // basic relay activation - break; - case 2: - throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - break; - case 3: - distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - break; - case 4: - NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - break; - } -} - -// de-activate the trigger after some delay, but without using a delay() function -void -Camera::trigger_pic_cleanup() -{ - if (keep_cam_trigg_active_cycles) - { - keep_cam_trigg_active_cycles --; - } - else - { - switch (trigger_type) - { - case 0: - case 2: - case 3: - if (rc_function[CAM_TRIGGER]) - { - cam_trigger = rc_function[CAM_TRIGGER]->radio_min; - } - break; - case 1: - relay_off(); - break; - case 4: - digitalWrite(camtrig, LOW); - break; - } - } -} From b7a0d8836adc7c9c856d3e8966b5d0a98b099be7 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Fri, 9 Sep 2011 16:18:38 +0200 Subject: [PATCH 05/74] This is the real HEAD of the APM_Camera branch. Seams that lots of changes got lost in the SVN to GIT port --- ArduPlane/APM_Config.h | 30 +- ArduPlane/APM_Config.h.reference | 23 +- ArduPlane/ArduPlane.pde | 373 ++++++------ ArduPlane/Attitude.pde | 133 +++-- ArduPlane/GCS.h | 8 +- ArduPlane/GCS_Mavlink.pde | 486 ++++++++++------ ArduPlane/HIL.h | 8 +- ArduPlane/HIL_Xplane.pde | 2 +- ArduPlane/Log.pde | 107 ++-- ArduPlane/Mavlink_Common.h | 158 +++++- ArduPlane/Parameters.h | 847 +++++++++++++++------------- ArduPlane/WP_activity.pde | 39 +- ArduPlane/climb_rate.pde | 44 +- ArduPlane/command description.txt | 2 +- ArduPlane/commands.pde | 103 ++-- ArduPlane/commands_logic.pde | 108 ++-- ArduPlane/commands_process.pde | 20 +- ArduPlane/config.h | 104 +++- ArduPlane/control_modes.pde | 73 +-- ArduPlane/defines.h | 47 +- ArduPlane/events.pde | 26 +- ArduPlane/navigation.pde | 57 +- ArduPlane/planner.pde | 9 +- ArduPlane/radio.pde | 139 ++++- ArduPlane/sensors.pde | 40 +- ArduPlane/setup.pde | 145 +++-- ArduPlane/system.pde | 170 +++--- ArduPlane/test.pde | 135 +++-- libraries/AP_Mount/AP_Mount.cpp | 213 +++---- libraries/AP_Mount/AP_Mount.h | 83 ++- libraries/RC_Channel/RC_Channel.cpp | 35 ++ libraries/RC_Channel/RC_Channel.h | 45 +- 32 files changed, 2253 insertions(+), 1559 deletions(-) diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 4006d33839..4ebb342fd1 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -23,35 +23,13 @@ #define GCS_PORT 3 */ -#define MAGNETOMETER ENABLED // ----- Camera definitions ------ // ------------------------------ #define CAMERA ENABLED #define CAM_DEBUG DISABLED -// Comment out servos that you do not have -//#define CAM_SERVO 8 // Camera servo channel -#define CAM_ANGLE 30 // Set angle in degrees -//#define CAM_CLICK 45 // This is the position of the servo arm when it actually clicks -//#define OPEN_SERVO 5 // Retraction servo channel - my camera retracts yours might not. -// Camera yaw (left-right) -#define YAW_SERVO 7 // Pan servo channel (can be pitch in stabilization) -#define YAW_REV 1 // output is normal = 1 output is reversed = -1 -#define YAW_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg -#define YAW_RANGE 90 // Pan Servo sweep in degrees -#define YAW_RATIO 10.31 // match this to the swing of your pan servo - -// Camera pitch (up-down) -#define PITCH_SERVO 6 // Tilt servo channel (can be roll in stabilization) -#define PITCH_REV 1 // output is normal = 1 output is reversed = -1 -#define PITCH_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg -#define PITCH_RANGE 90 // Tilt Servo sweep in degrees -#define PITCH_RATIO 10.31 // match this to the swing of your tilt servo - -// Camera roll (up-down) -#define ROLL_SERVO 6 // Tilt servo channel (can be roll in stabilization) -#define ROLL_REV 1 // output is normal = 1 output is reversed = -1 -#define ROLL_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg -#define ROLL_RANGE 90 // Tilt Servo sweep in degrees -#define ROLL_RATIO 10.31 // match this to the swing of your tilt servo +// - Options to reduce code size - +// ------------------------------- +// Disable text based terminal configuration +#define CLI_ENABLED DISABLED diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 18fa27d019..3786839dc6 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -210,7 +210,7 @@ // INPUT_VOLTAGE OPTIONAL // // In order to have accurate pressure and battery voltage readings, this -// value should be set to the voltage measured on the 5V rail on the oilpan. +// value should be set to the voltage measured at the processor. // // See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail. // @@ -304,6 +304,19 @@ //#define FLIGHT_MODE_6 MANUAL // +////////////////////////////////////////////////////////////////////////////// +// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed +// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed +// then the flap position shall be flap_1_percent. If the speed setpoint is below flap_2_speed +// then the flap position shall be flap_2_percent. Speed setpoint is the current value of +// airspeed_cruise (if airspeed sensor enabled) or throttle_cruise (if no airspeed sensor) + +// FLAP_1_PERCENT OPTIONAL +// FLAP_1_SPEED OPTIONAL +// FLAP_2_PERCENT OPTIONAL +// FLAP_2_SPEED OPTIONAL + + ////////////////////////////////////////////////////////////////////////////// // THROTTLE_FAILSAFE OPTIONAL // THROTTLE_FS_VALUE OPTIONAL @@ -361,7 +374,7 @@ // the aircraft will head for home in RTL mode. If the failsafe condition is // resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home -// If XX_FAILSAFE_ACTION is 2 and the applicable failsafe occurs while in AUTO or LOITER +// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER // mode the aircraft will continue in that mode ignoring the failsafe condition. // Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always @@ -372,8 +385,8 @@ // The default behaviour is to ignore failsafes in AUTO and LOITER modes. // //#define GCS_HEARTBEAT_FAILSAFE DISABLED -//#define SHORT_FAILSAFE_ACTION 2 -//#define LONG_FAILSAFE_ACTION 2 +//#define SHORT_FAILSAFE_ACTION 0 +//#define LONG_FAILSAFE_ACTION 0 ////////////////////////////////////////////////////////////////////////////// @@ -774,7 +787,7 @@ // // Limits the slew rate of the throttle, in percent per second. Helps // avoid sudden throttle changes, which can destabilise the aircraft. -// A setting of zero disables the feature. +// A setting of zero disables the feature. Range 1 to 100. // Default is zero (disabled). // // P_TO_T OPTIONAL diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 8a43a61634..2b744873a2 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega 2.2.0" +#define THISFIRMWARE "ArduPilotMega V2.23" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -29,6 +29,7 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega RC Library #include // ArduPilot GPS library #include // Arduino I2C lib +#include // Arduino SPI lib #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library #include // ArduPilot Mega BMP085 Library @@ -38,10 +39,13 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library -//#include // Range finder library +#include // Range finder library +#include +#include // Photo or video camera +#include // Camera mount -#define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions +#include // Configuration #include "config.h" @@ -70,12 +74,12 @@ FastSerialPort3(Serial3); // Telemetry port // // Global parameters are all contained within the 'g' class. // -Parameters g; +static Parameters g; //////////////////////////////////////////////////////////////////////////////// // prototypes -void update_events(void); +static void update_events(void); //////////////////////////////////////////////////////////////////////////////// @@ -92,23 +96,17 @@ void update_events(void); // // All GPS access should be through this pointer. -GPS *g_gps; +static GPS *g_gps; + +// flight modes convenience array +static AP_Int8 *flight_modes = &g.flight_mode1; #if HIL_MODE == HIL_MODE_DISABLED // real sensors -AP_ADC_ADS7844 adc; -APM_BMP085_Class barometer; -// MAG PROTOCOL -#if MAG_PROTOCOL == MAG_PROTOCOL_5843 -AP_Compass_HMC5843 compass(Parameters::k_param_compass); - -#elif MAG_PROTOCOL == MAG_PROTOCOL_5883L -AP_Compass_HMC5883L compass(Parameters::k_param_compass); - -#else - #error Unrecognised MAG_PROTOCOL setting. -#endif // MAG PROTOCOL +static AP_ADC_ADS7844 adc; +static APM_BMP085_Class barometer; +static AP_Compass_HMC5843 compass(Parameters::k_param_compass); // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -144,6 +142,7 @@ AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); #elif HIL_MODE == HIL_MODE_ATTITUDE +AP_ADC_HIL adc; AP_DCM_HIL dcm; AP_GPS_HIL g_gps_driver(NULL); AP_Compass_HIL compass; // never used @@ -189,18 +188,31 @@ AP_IMU_Shim imu; // never used GCS_Class gcs; #endif -//AP_RangeFinder_MaxsonarXL sonar; +//////////////////////////////////////////////////////////////////////////////// +// SONAR selection +//////////////////////////////////////////////////////////////////////////////// +// +ModeFilter sonar_mode_filter; + +#if SONAR_TYPE == MAX_SONAR_XL + AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); +#elif SONAR_TYPE == MAX_SONAR_LV + // XXX honestly I think these output the same values + // If someone knows, can they confirm it? + AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); +#endif //////////////////////////////////////////////////////////////////////////////// // Global variables //////////////////////////////////////////////////////////////////////////////// -byte control_mode = MANUAL; +byte control_mode = INITIALISING; byte oldSwitchPosition; // for remembering the control mode switch +bool inverted_flight = false; -const char *comma = ","; +static const char *comma = ","; -const char* flight_mode_strings[] = { +static const char* flight_mode_strings[] = { "Manual", "Circle", "Stabilize", @@ -232,221 +244,196 @@ const char* flight_mode_strings[] = { // Failsafe // -------- -int failsafe; // track which type of failsafe is being processed -bool ch3_failsafe; -byte crash_timer; +static int failsafe; // track which type of failsafe is being processed +static bool ch3_failsafe; +static byte crash_timer; // Radio // ----- -uint16_t elevon1_trim = 1500; // TODO: handle in EEProm -uint16_t elevon2_trim = 1500; -uint16_t ch1_temp = 1500; // Used for elevon mixing -uint16_t ch2_temp = 1500; -int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; -bool rc_override_active = false; -uint32_t rc_override_fs_timer = 0; -uint32_t ch3_failsafe_timer = 0; +static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm +static uint16_t elevon2_trim = 1500; +static uint16_t ch1_temp = 1500; // Used for elevon mixing +static uint16_t ch2_temp = 1500; +static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; +static bool rc_override_active = false; +static uint32_t rc_override_fs_timer = 0; +static uint32_t ch3_failsafe_timer = 0; -bool reverse_roll; -bool reverse_pitch; -bool reverse_rudder; -byte mix_mode; // 0 = normal , 1 = elevons - -// TODO: switch these reverses to true/false, after they are handled by RC_Channel -int reverse_elevons = 1; -int reverse_ch1_elevon = 1; -int reverse_ch2_elevon = 1; // for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon // LED output // ---------- -bool GPS_light; // status of the GPS light +static bool GPS_light; // status of the GPS light // GPS variables // ------------- -const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage -float scaleLongUp = 1; // used to reverse longtitude scaling -float scaleLongDown = 1; // used to reverse longtitude scaling -byte ground_start_count = 5; // have we achieved first lock and set Home? -int ground_start_avg; // 5 samples to avg speed for ground start -bool ground_start; // have we started on the ground? -bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present +static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage +static float scaleLongUp = 1; // used to reverse longitude scaling +static float scaleLongDown = 1; // used to reverse longitude scaling +static byte ground_start_count = 5; // have we achieved first lock and set Home? +static int ground_start_avg; // 5 samples to avg speed for ground start +static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present // Location & Navigation // --------------------- const float radius_of_earth = 6378100; // meters const float gravity = 9.81; // meters/ sec^2 -long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate -long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target -int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate -float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? -long hold_course = -1; // deg * 100 dir of plane +static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target +static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate +static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? +static long hold_course = -1; // deg * 100 dir of plane -byte command_must_index; // current command memory location -byte command_may_index; // current command memory location -byte command_must_ID; // current command ID -byte command_may_ID; // current command ID +static byte command_must_index; // current command memory location +static byte command_may_index; // current command memory location +static byte command_must_ID; // current command ID +static byte command_may_ID; // current command ID // Airspeed // -------- -int airspeed; // m/s * 100 -int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -float airspeed_error; // m/s * 100 -long energy_error; // energy state error (kinetic + potential) for altitude hold -long airspeed_energy_error; // kinetic portion of energy error -bool airspeed_enabled = false; +static int airspeed; // m/s * 100 +static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range +static float airspeed_error; // m/s * 100 +static long energy_error; // energy state error (kinetic + potential) for altitude hold +static long airspeed_energy_error; // kinetic portion of energy error // Location Errors // --------------- -long bearing_error; // deg * 100 : 0 to 36000 -long altitude_error; // meters * 100 we are off in altitude -float crosstrack_error; // meters we are off trackline +static long bearing_error; // deg * 100 : 0 to 36000 +static long altitude_error; // meters * 100 we are off in altitude +static float crosstrack_error; // meters we are off trackline // Battery Sensors // --------------- -float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter -float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter -float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter +static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter +static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter +static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter +static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter +static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter -float current_amps; -float current_total; +static float current_amps; +static float current_total; // Airspeed Sensors // ---------------- -float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering -int airspeed_offset; // analog air pressure sensor while still -int airspeed_pressure; // airspeed as a pressure value +static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering +static int airspeed_pressure; // airspeed as a pressure value // Barometer Sensor variables // -------------------------- -unsigned long abs_pressure; +static unsigned long abs_pressure; // Altitude Sensor variables // ---------------------- -//byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR +static int sonar_alt; // flight mode specific // -------------------- -bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. -bool land_complete; -long takeoff_altitude; -int landing_distance; // meters; -int landing_pitch; // pitch for landing set by commands -int takeoff_pitch; +static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. +static bool land_complete; +static long takeoff_altitude; +// static int landing_distance; // meters; +static int landing_pitch; // pitch for landing set by commands +static int takeoff_pitch; // Loiter management // ----------------- -long old_target_bearing; // deg * 100 -int loiter_total; // deg : how many times to loiter * 360 -int loiter_delta; // deg : how far we just turned -int loiter_sum; // deg : how far we have turned around a waypoint -long loiter_time; // millis : when we started LOITER mode -int loiter_time_max; // millis : how long to stay in LOITER mode +static long old_target_bearing; // deg * 100 +static int loiter_total; // deg : how many times to loiter * 360 +static int loiter_delta; // deg : how far we just turned +static int loiter_sum; // deg : how far we have turned around a waypoint +static long loiter_time; // millis : when we started LOITER mode +static int loiter_time_max; // millis : how long to stay in LOITER mode // these are the values for navigation control functions // ---------------------------------------------------- -long nav_roll; // deg * 100 : target roll angle -long nav_pitch; // deg * 100 : target pitch angle -int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel +static long nav_roll; // deg * 100 : target roll angle +static long nav_pitch; // deg * 100 : target pitch angle +static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel // Waypoints // --------- -long wp_distance; // meters - distance between plane and next waypoint -long wp_totalDistance; // meters - distance between old and next waypoint -byte next_wp_index; // Current active command index +static long wp_distance; // meters - distance between plane and next waypoint +static long wp_totalDistance; // meters - distance between old and next waypoint // repeating event control // ----------------------- -byte event_id; // what to do - see defines -long event_timer; // when the event was asked for in ms -uint16_t event_delay; // how long to delay the next firing of event in millis -int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles -int event_value; // per command value, such as PWM for servos -int event_undo_value; // the value used to cycle events (alternate value to event_value) +static byte event_id; // what to do - see defines +static long event_timer; // when the event was asked for in ms +static uint16_t event_delay; // how long to delay the next firing of event in millis +static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles +static int event_value; // per command value, such as PWM for servos +static int event_undo_value; // the value used to cycle events (alternate value to event_value) // delay command // -------------- -long condition_value; // used in condition commands (eg delay, change alt, etc.) -long condition_start; -int condition_rate; +static long condition_value; // used in condition commands (eg delay, change alt, etc.) +static long condition_start; +static int condition_rate; // 3D Location vectors // ------------------- -struct Location home; // home location -struct Location prev_WP; // last waypoint -struct Location current_loc; // current location -struct Location next_WP; // next waypoint -struct Location next_command; // command preloaded -long target_altitude; // used for altitude management between waypoints -long offset_altitude; // used for altitude management between waypoints -bool home_is_set; // Flag for if we have g_gps lock and have set the home location +static struct Location home; // home location +static struct Location prev_WP; // last waypoint +static struct Location current_loc; // current location +static struct Location next_WP; // next waypoint +static struct Location next_command; // command preloaded +static struct Location guided_WP; // guided mode waypoint +static long target_altitude; // used for altitude management between waypoints +static long offset_altitude; // used for altitude management between waypoints +static bool home_is_set; // Flag for if we have g_gps lock and have set the home location // IMU variables // ------------- -float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) +static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) // Performance monitoring // ---------------------- -long perf_mon_timer; // Metric based on accel gain deweighting -int G_Dt_max; // Max main loop cycle time in milliseconds -int gps_fix_count; -byte gcs_messages_sent; +static long perf_mon_timer; // Metric based on accel gain deweighting +static int G_Dt_max = 0; // Max main loop cycle time in milliseconds +static int gps_fix_count = 0; +static int pmTest1 = 0; -// GCS -// --- -char GCS_buffer[53]; -char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed - // System Timers // -------------- -unsigned long fast_loopTimer; // Time in miliseconds of main control loop -unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete -uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -int mainLoop_count; +static unsigned long fast_loopTimer; // Time in miliseconds of main control loop +static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete +static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds +static int mainLoop_count; -unsigned long medium_loopTimer; // Time in miliseconds of medium loop -byte medium_loopCounter; // Counters for branching from main control loop to slower loops -uint8_t delta_ms_medium_loop; +static unsigned long medium_loopTimer; // Time in miliseconds of medium loop +static byte medium_loopCounter; // Counters for branching from main control loop to slower loops +static uint8_t delta_ms_medium_loop; -byte slow_loopCounter; -byte superslow_loopCounter; -byte counter_one_herz; +static byte slow_loopCounter; +static byte superslow_loopCounter; +static byte counter_one_herz; -unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav +static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav -unsigned long dTnav; // Delta Time in milliseconds for navigation computations -unsigned long elapsedTime; // for doing custom events -float load; // % MCU cycles used +static unsigned long dTnav; // Delta Time in milliseconds for navigation computations +static float load; // % MCU cycles used + +RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function //Camera tracking and stabilisation stuff // -------------------------------------- -byte camera_mode = 1; //0 is do nothing, 1 is stabilize, 2 is track target -byte gimbal_mode = 0; // 0 - pitch & roll, 1 - pitch and yaw (pan & tilt), 2 - pitch, roll and yaw (to be added) -struct Location camera_target; //point of iterest for the camera to track -Vector3 target_vector(0,0,1); //x, y, z to target before rotating to planes axis, values are in meters -float cam_pitch; -float cam_roll; -float cam_tilt; -float cam_pan; - -struct Location GPS_mark; // GPS POI for position based triggering -int picture_time = 0; // waypoint trigger variable -int thr_pic = 0; // timer variable for throttle_pic -int camtrig = 83; // PK6 chosen as it not near anything so safer for soldering -pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) +#if CAMERA == ENABLED +AP_Mount camera_mount(g_gps, &dcm); +//pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) +#endif //////////////////////////////////////////////////////////////////////////////// // Top-level logic //////////////////////////////////////////////////////////////////////////////// void setup() { + memcheck_init(); init_ardupilot(); } @@ -491,7 +478,7 @@ void loop() } // Main loop 50Hz -void fast_loop() +static void fast_loop() { // This is the fast loop - we want it to execute at 50Hz if possible // ----------------------------------------------------------------- @@ -502,15 +489,22 @@ void fast_loop() // ---------- read_radio(); + // try to send any deferred messages if the serial port now has + // some space available + gcs.send_message(MSG_RETRY_DEFERRED); +#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) + hil.send_message(MSG_RETRY_DEFERRED); +#endif + // check for loss of control signal failsafe condition // ------------------------------------ check_short_failsafe(); // Read Airspeed // ------------- - if (airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { + if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { read_airspeed(); - } else if (airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { + } else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { calc_airspeed_errors(); } @@ -550,7 +544,7 @@ void fast_loop() // write out the servo PWM values // ------------------------------ - set_servos_4(); + set_servos(); // XXX is it appropriate to be doing the comms below on the fast loop? @@ -579,9 +573,14 @@ void fast_loop() // or be a "GCS fast loop" interface } -void medium_loop() +static void medium_loop() { - camera(); +#if CAMERA == ENABLED + // TODO replace home with a POI coming from a MavLink message or command + camera_mount.set_GPS_target(home); + g.camera.trigger_pic_cleanup(); +#endif + // This is the start of the medium (10 Hz) loop pieces // ----------------------------------------- switch(medium_loopCounter) { @@ -614,7 +613,6 @@ Serial.println(tempaccel.z, DEC); // This case performs some navigation computations //------------------------------------------------ case 1: - medium_loopCounter++; @@ -638,6 +636,7 @@ Serial.println(tempaccel.z, DEC); // Read altitude from sensors // ------------------ update_alt(); + if(g.sonar_enabled) sonar_alt = sonar.read(); // altitude smoothing // ------------------ @@ -698,7 +697,7 @@ Serial.println(tempaccel.z, DEC); } } -void slow_loop() +static void slow_loop() { // This is the slow (3 1/3 Hz) loop pieces //---------------------------------------- @@ -727,9 +726,9 @@ void slow_loop() // Read Control Surfaces/Mix switches // ---------------------------------- - if (g.switch_enable) { - update_servo_switches(); - } + update_servo_switches(); + + update_aux_servo_function(); break; @@ -737,19 +736,18 @@ void slow_loop() slow_loopCounter = 0; update_events(); - // XXX this should be a "GCS slow loop" interface #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter - gcs.data_stream_send(1,5); + gcs.data_stream_send(3,5); // send all requested output streams with rates requested - // between 1 and 5 Hz + // between 3 and 5 Hz #else gcs.send_message(MSG_LOCATION); gcs.send_message(MSG_CPU_LOAD, load*100); #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(1,5); + hil.data_stream_send(3,5); #endif @@ -757,20 +755,26 @@ void slow_loop() } } -void one_second_loop() +static void one_second_loop() { if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); // send a heartbeat gcs.send_message(MSG_HEARTBEAT); + #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK + gcs.data_stream_send(1,3); + // send all requested output streams with rates requested + // between 1 and 3 Hz + #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_HEARTBEAT); + hil.data_stream_send(1,3); #endif } -void update_GPS(void) +static void update_GPS(void) { g_gps->update(); update_GPS_light(); @@ -822,7 +826,7 @@ void update_GPS(void) } } -void update_current_flight_mode(void) +static void update_current_flight_mode(void) { if(control_mode == AUTO){ crash_checker(); @@ -835,7 +839,7 @@ void update_current_flight_mode(void) nav_roll = 0; } - if (airspeed_enabled == true) + if (g.airspeed_enabled == true) { calc_nav_pitch(); if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; @@ -853,7 +857,7 @@ void update_current_flight_mode(void) case MAV_CMD_NAV_LAND: calc_nav_roll(); - if (airspeed_enabled == true){ + if (g.airspeed_enabled == true){ calc_nav_pitch(); calc_throttle(); }else{ @@ -878,6 +882,7 @@ void update_current_flight_mode(void) switch(control_mode){ case RTL: case LOITER: + case GUIDED: hold_course = -1; crash_checker(); calc_nav_roll(); @@ -891,6 +896,7 @@ void update_current_flight_mode(void) nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min; // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority + if (inverted_flight) nav_pitch = -nav_pitch; break; case FLY_BY_WIRE_B: @@ -900,7 +906,7 @@ void update_current_flight_mode(void) nav_roll = g.channel_roll.norm_input() * g.roll_limit; altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; - if (airspeed_enabled == true) + if (g.airspeed_enabled == true) { airspeed_error = ((int)(g.flybywire_airspeed_max - g.flybywire_airspeed_min) * @@ -949,7 +955,7 @@ void update_current_flight_mode(void) } } -void update_navigation() +static void update_navigation() { // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS // ------------------------------------------------------------------------ @@ -961,23 +967,18 @@ void update_navigation() switch(control_mode){ case LOITER: + case RTL: + case GUIDED: update_loiter(); calc_bearing_error(); break; - case RTL: - if(wp_distance <= ( g.loiter_radius + LOITER_RANGE) ) { - do_RTL(); - }else{ - update_crosstrack(); - } - break; } } } -void update_alt() +static void update_alt() { #if HIL_MODE == HIL_MODE_ATTITUDE current_loc.alt = g_gps->altitude; @@ -990,6 +991,6 @@ void update_alt() #endif // Calculate new climb rate - if(medium_loopCounter == 0 && slow_loopCounter == 0) - add_altitude_data(millis() / 100, g_gps->altitude / 10); + //if(medium_loopCounter == 0 && slow_loopCounter == 0) + // add_altitude_data(millis() / 100, g_gps->altitude / 10); } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index b942ec759d..e5a109744d 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -4,15 +4,14 @@ // Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. //**************************************************************** -void stabilize() +static void stabilize() { - static byte temp = 0; float ch1_inf = 1.0; float ch2_inf = 1.0; float ch4_inf = 1.0; float speed_scaler; - if (airspeed_enabled == true){ + if (g.airspeed_enabled == true){ if(airspeed > 0) speed_scaler = (STANDARD_SPEED * 100) / airspeed; else @@ -20,7 +19,7 @@ void stabilize() speed_scaler = constrain(speed_scaler, 0.5, 2.0); } else { if (g.channel_throttle.servo_out > 0){ - speed_scaler = 0.5 + (THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root + speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root // Should maybe be to the 2/7 power, but we aren't goint to implement that... }else{ speed_scaler = 1.67; @@ -32,6 +31,16 @@ void stabilize() nav_roll = 0; } + if (inverted_flight) { + // we want to fly upside down. We need to cope with wrap of + // the roll_sensor interfering with wrap of nav_roll, which + // would really confuse the PID code. The easiest way to + // handle this is to ensure both go in the same direction from + // zero + nav_roll += 18000; + if (dcm.roll_sensor < 0) nav_roll -= 36000; + } + // For Testing Only // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; // Serial.printf_P(PSTR(" roll_sensor ")); @@ -44,6 +53,10 @@ void stabilize() fabs(dcm.roll_sensor * g.kff_pitch_compensation) + (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - (dcm.pitch_sensor - g.pitch_trim); + if (inverted_flight) { + // when flying upside down the elevator control is inverted + tempcalc = -tempcalc; + } g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); // Mix Stick input to allow users to override control surfaces @@ -99,7 +112,7 @@ void stabilize() //#endif } -void crash_checker() +static void crash_checker() { if(dcm.pitch_sensor < -4500){ crash_timer = 255; @@ -109,9 +122,9 @@ void crash_checker() } -void calc_throttle() +static void calc_throttle() { - if (airspeed_enabled == false) { + if (g.airspeed_enabled == false) { int throttle_target = g.throttle_cruise + throttle_nudge; // no airspeed sensor, we use nav pitch to determine the proper throttle output @@ -145,7 +158,7 @@ void calc_throttle() // Yaw is separated into a function for future implementation of heading hold on rolling take-off // ---------------------------------------------------------------------------------------- -void calc_nav_yaw(float speed_scaler) +static void calc_nav_yaw(float speed_scaler) { #if HIL_MODE != HIL_MODE_ATTITUDE Vector3f temp = imu.get_accel(); @@ -160,11 +173,11 @@ void calc_nav_yaw(float speed_scaler) } -void calc_nav_pitch() +static void calc_nav_pitch() { // Calculate the Pitch of the plane // -------------------------------- - if (airspeed_enabled == true) { + if (g.airspeed_enabled == true) { nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); } else { nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); @@ -175,7 +188,7 @@ void calc_nav_pitch() #define YAW_DAMPENER 0 -void calc_nav_roll() +static void calc_nav_roll() { // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. @@ -219,18 +232,22 @@ float roll_slew_limit(float servo) /***************************************** * Throttle slew limit *****************************************/ -/*float throttle_slew_limit(float throttle) +static void throttle_slew_limit() { - static float last; - float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f); - last = throttle; - return temp; + static int last = 1000; + if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit + + float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000 +Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last); + g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp); + last = g.channel_throttle.radio_out; + } } -*/ + // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -void reset_I(void) +static void reset_I(void) { g.pidNavRoll.reset_I(); g.pidNavPitchAirspeed.reset_I(); @@ -242,11 +259,13 @@ void reset_I(void) /***************************************** * Set the flight control servos based on the current calculated values *****************************************/ -void set_servos_4(void) +static void set_servos(void) { + int flapSpeedSource = 0; + if(control_mode == MANUAL){ // do a direct pass through of radio values - if (mix_mode == 0){ + if (g.mix_mode == 0){ g.channel_roll.radio_out = g.channel_roll.radio_in; g.channel_pitch.radio_out = g.channel_pitch.radio_in; } else { @@ -255,21 +274,26 @@ void set_servos_4(void) } g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_rudder.radio_out = g.channel_rudder.radio_in; + if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; } else { - if (mix_mode == 0){ + if (g.mix_mode == 0) { g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); g.channel_rudder.calc_pwm(); + if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) { + g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out; + g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); + } }else{ /*Elevon mode*/ float ch1; float ch2; - ch1 = reverse_elevons * (g.channel_pitch.servo_out - g.channel_roll.servo_out); + ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out); ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out; - g.channel_roll.radio_out = elevon1_trim + (reverse_ch1_elevon * (ch1 * 500.0/ ROLL_SERVO_MAX)); - g.channel_pitch.radio_out = elevon2_trim + (reverse_ch2_elevon * (ch2 * 500.0/ PITCH_SERVO_MAX)); + g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); + g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); } #if THROTTLE_OUT == 0 @@ -281,55 +305,60 @@ void set_servos_4(void) g.channel_throttle.calc_pwm(); - //Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938 - /* TO DO - fix this for RC_Channel library #if THROTTLE_REVERSE == 1 radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE]; #endif */ + + throttle_slew_limit(); } - - + + if(control_mode <= FLY_BY_WIRE_B) { + if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; + } else if (control_mode >= FLY_BY_WIRE_C) { + if (g.airspeed_enabled == true) { + flapSpeedSource = g.airspeed_cruise; + } else { + flapSpeedSource = g.throttle_cruise; + } + if ( flapSpeedSource > g.flap_1_speed) { + if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = 0; + } else if (flapSpeedSource > g.flap_2_speed) { + if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_1_percent; + } else { + if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_2_percent; + } + } + +#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS // send values to the PWM timers for output // ---------------------------------------- APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos + // Route configurable aux. functions to their respective servos + aux_servo_out(&g.rc_5, CH_5); + aux_servo_out(&g.rc_6, CH_6); + aux_servo_out(&g.rc_7, CH_7); + aux_servo_out(&g.rc_8, CH_8); +#endif } -void demo_servos(byte i) { +static void demo_servos(byte i) { while(i > 0){ gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); +#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS APM_RC.OutputCh(1, 1400); - delay(400); + mavlink_delay(400); APM_RC.OutputCh(1, 1600); - delay(200); + mavlink_delay(200); APM_RC.OutputCh(1, 1500); - delay(400); +#endif + mavlink_delay(400); i--; } } -int readOutputCh(unsigned char ch) -{ - int pwm; - switch(ch) - { - case 0: pwm = OCR5B; break; // ch0 - case 1: pwm = OCR5C; break; // ch1 - case 2: pwm = OCR1B; break; // ch2 - case 3: pwm = OCR1C; break; // ch3 - case 4: pwm = OCR4C; break; // ch4 - case 5: pwm = OCR4B; break; // ch5 - case 6: pwm = OCR3C; break; // ch6 - case 7: pwm = OCR3B; break; // ch7 - case 8: pwm = OCR5A; break; // ch8, PL3 - case 9: pwm = OCR1A; break; // ch9, PB5 - case 10: pwm = OCR3A; break; // ch10, PE3 - } - pwm >>= 1; // pwm / 2; - return pwm; -} diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index a80673966e..a654d823eb 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -6,7 +6,7 @@ #ifndef __GCS_H #define __GCS_H -#include +#include #include #include #include @@ -40,7 +40,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(BetterStream *port) { _port = port; } + void init(FastSerial *port) { _port = port; } /// Update GCS state. /// @@ -119,7 +119,7 @@ public: protected: /// The stream we are communicating over - BetterStream *_port; + FastSerial *_port; }; // @@ -139,7 +139,7 @@ class GCS_MAVLINK : public GCS_Class public: GCS_MAVLINK(AP_Var::Key key); void update(void); - void init(BetterStream *port); + void init(FastSerial *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const prog_char_t *str); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index b578aedcca..db34c15389 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -4,6 +4,10 @@ #include "Mavlink_Common.h" +// use this to prevent recursion during sensor init +static bool in_mavlink_delay; + + GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : packet_drops(0), @@ -14,21 +18,21 @@ waypoint_receive_timeout(1000), // 1 second // stream rates _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), -streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), -streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), -streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), -streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), -streamRatePosition (&_group, 4, 0, PSTR("POSITION")), -streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), -streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), -streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) - -{ + // AP_VAR //ref //index, default, name + streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), + streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), + streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), + streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), + streamRatePosition (&_group, 4, 0, PSTR("POSITION")), + streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), + streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), + streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) +{ } void -GCS_MAVLINK::init(BetterStream * port) +GCS_MAVLINK::init(FastSerial * port) { GCS_Class::init(port); if (port == &Serial) { // to split hil vs gcs @@ -53,8 +57,6 @@ GCS_MAVLINK::update(void) { uint8_t c = comm_receive_ch(chan); - - // Try to get a new message if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); } @@ -62,11 +64,6 @@ GCS_MAVLINK::update(void) // Update packet drops counter packet_drops += status.packet_rx_drop_count; - - - - - // send out queued params/ waypoints _queued_send(); @@ -87,41 +84,55 @@ void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { - if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax)) - send_message(MSG_RAW_IMU); - if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) { - send_message(MSG_EXTENDED_STATUS); + if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ + send_message(MSG_RAW_IMU1); + send_message(MSG_RAW_IMU2); + send_message(MSG_RAW_IMU3); + //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); + } + + if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { + send_message(MSG_EXTENDED_STATUS1); + send_message(MSG_EXTENDED_STATUS2); send_message(MSG_GPS_STATUS); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); + //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); } - if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) { + if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { + // sent with GPS read send_message(MSG_LOCATION); + //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); } - if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) { + if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { // This is used for HIL. Do not change without discussing with HIL maintainers send_message(MSG_SERVO_OUT); + //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); } - if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) { + if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); + //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); } - if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info + if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info send_message(MSG_ATTITUDE); + //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); } - if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info + if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info send_message(MSG_VFR_HUD); + //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); } - if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){ + if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ // Available datastream + //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } @@ -129,7 +140,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) void GCS_MAVLINK::send_message(uint8_t id, uint32_t param) { - mavlink_send_message(chan,id,param,packet_drops); + mavlink_send_message(chan,id, packet_drops); } void @@ -158,9 +169,9 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { - struct Location tell_command; // command for telemetry + struct Location tell_command = {}; // command for telemetry static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...) - + switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: @@ -168,13 +179,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; int freq = 0; // packet frequency - if (packet.start_stop == 0) freq = 0; // stop sending - else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending - else break; + if (packet.start_stop == 0) + freq = 0; // stop sending + else if (packet.start_stop == 1) + freq = packet.req_message_rate; // start sending + else + break; switch(packet.req_stream_id){ @@ -188,6 +204,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) streamRateExtra2 = freq; streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. break; + case MAV_DATA_STREAM_RAW_SENSORS: streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly // we will not continue to broadcast raw sensor data at 50Hz. @@ -195,27 +212,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_DATA_STREAM_EXTENDED_STATUS: streamRateExtendedStatus.set_and_save(freq); break; + case MAV_DATA_STREAM_RC_CHANNELS: streamRateRCChannels.set_and_save(freq); break; + case MAV_DATA_STREAM_RAW_CONTROLLER: streamRateRawController.set_and_save(freq); break; - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; + + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; + case MAV_DATA_STREAM_POSITION: streamRatePosition.set_and_save(freq); break; + case MAV_DATA_STREAM_EXTRA1: streamRateExtra1.set_and_save(freq); break; + case MAV_DATA_STREAM_EXTRA2: streamRateExtra2.set_and_save(freq); break; + case MAV_DATA_STREAM_EXTRA3: streamRateExtra3.set_and_save(freq); break; + default: break; } @@ -228,11 +253,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_action_t packet; mavlink_msg_action_decode(msg, &packet); if (mavlink_check_target(packet.target,packet.target_component)) break; - + uint8_t result = 0; // do action - send_text_P(SEVERITY_LOW,PSTR("action received: ")); + send_text_P(SEVERITY_LOW,PSTR("action received: ")); //Serial.println(packet.action); switch(packet.action){ @@ -332,13 +357,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) default: break; } - + mavlink_msg_action_ack_send( chan, packet.action, result ); - + break; } @@ -347,35 +372,38 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg, &packet); - + switch(packet.mode){ case MAV_MODE_MANUAL: set_mode(MANUAL); break; - - case MAV_MODE_GUIDED: //For future use + + case MAV_MODE_GUIDED: + set_mode(GUIDED); break; - + case MAV_MODE_AUTO: if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); mav_nav = 255; break; - + case MAV_MODE_TEST1: set_mode(STABILIZE); break; + case MAV_MODE_TEST2: - set_mode(FLY_BY_WIRE_A); - break; - case MAV_MODE_TEST3: - set_mode(FLY_BY_WIRE_B); + if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A); + if(mav_nav == 2) set_mode(FLY_BY_WIRE_B); + //if(mav_nav == 3) set_mode(FLY_BY_WIRE_C); + mav_nav = 255; break; + } } - + case MAVLINK_MSG_ID_SET_NAV_MODE: { // decode @@ -385,7 +413,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mav_nav = packet.nav_mode; break; } - + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); @@ -393,7 +421,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_list_t packet; mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; // Start sending waypoints mavlink_msg_waypoint_count_send( @@ -410,6 +439,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); @@ -421,28 +451,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_t packet; mavlink_msg_waypoint_request_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; // send waypoint tell_command = get_wp_with_index(packet.seq); // set frame of waypoint uint8_t frame; - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame } else { frame = MAV_FRAME_GLOBAL; // reference frame } - + float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) - if (packet.seq == g.waypoint_index) current = 1; + + if (packet.seq == (uint16_t)g.waypoint_index) + current = 1; + uint8_t autocontinue = 1; // 1 (true), 0 (false) + float x = 0, y = 0, z = 0; - if (tell_command.id < MAV_CMD_NAV_LAST) { + if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { // command needs scaling x = tell_command.lat/1.0e7; // local (x), global (latitude) y = tell_command.lng/1.0e7; // local (y), global (longitude) @@ -452,50 +489,63 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) } } - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_CONDITION_CHANGE_ALT: - case MAV_CMD_DO_SET_HOME: - param1 = tell_command.p1; - break; + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1*10; // APM loiter time is in ten second increments - break; + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_DO_SET_HOME: + param1 = tell_command.p1; + break; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1*10; // APM loiter time is in ten second increments + break; - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - param1 = tell_command.p1; - break; + case MAV_CMD_CONDITION_CHANGE_ALT: + x=0; // Clear fields loaded above that we don't want sent for this command + y=0; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - param3 = tell_command.lat; - param2 = tell_command.alt; - param1 = tell_command.p1; - break; + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - } + case MAV_CMD_DO_REPEAT_SERVO: + param4 = tell_command.lng; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + param3 = tell_command.lat; + param2 = tell_command.alt; + param1 = tell_command.p1; + break; - mavlink_msg_waypoint_send(chan,msg->sysid, - msg->compid,packet.seq,frame,tell_command.id,current,autocontinue, - param1,param2,param3,param4,x,y,z); + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + } + + mavlink_msg_waypoint_send(chan,msg->sysid, + msg->compid, + packet.seq, + frame, + tell_command.id, + current, + autocontinue, + param1, + param2, + param3, + param4, + x, + y, + z); // update last waypoint comm stamp waypoint_timelast_send = millis(); @@ -511,9 +561,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_ack_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; - // check for error - uint8_t type = packet.type; // ok (0), error(1) - // turn off waypoint send waypoint_sending = false; break; @@ -544,14 +591,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_clear_all_t packet; mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + if (mavlink_check_target(packet.target_system, packet.target_component)) break; // clear all waypoints uint8_t type = 0; // ok (0), error(1) g.waypoint_total.set_and_save(0); // send acknowledgement 3 times to makes sure it is received - for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); + for (int i=0;i<3;i++) + mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type); break; } @@ -586,6 +634,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) packet.count = MAX_WAYPOINTS; } g.waypoint_total.set_and_save(packet.count - 1); + waypoint_timelast_receive = millis(); waypoint_receiving = true; waypoint_sending = false; @@ -593,55 +642,58 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } +#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS + case MAVLINK_MSG_ID_SET_MAG_OFFSETS: + { + mavlink_set_mag_offsets_t packet; + mavlink_msg_set_mag_offsets_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); + break; + } +#endif + + // XXX receive a WP from GCS and store in EEPROM case MAVLINK_MSG_ID_WAYPOINT: { - // Check if receiving waypiont - if (!waypoint_receiving) break; - // decode mavlink_waypoint_t packet; mavlink_msg_waypoint_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) break; - - // store waypoint - uint8_t loadAction = 0; // 0 insert in list, 1 exec now - // defaults tell_command.id = packet.command; - switch (packet.frame) - { - case MAV_FRAME_MISSION: - case MAV_FRAME_GLOBAL: - { - 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 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm - tell_command.options = 0; - break; - } + switch (packet.frame) + { + case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: + { + 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 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.options = 0; // absolute altitude + break; + } - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; - tell_command.options = 1; - break; - } - case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - { - 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 - tell_command.alt = packet.z*1.0e2; - tell_command.options = 1; - break; - } - } + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + { + 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 + tell_command.alt = packet.z * 1.0e2; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + break; + } + } switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields case MAV_CMD_NAV_LOITER_TURNS: @@ -651,7 +703,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.p1 = packet.param1 * 100; + tell_command.lat = packet.param1; break; case MAV_CMD_NAV_LOITER_TIME: @@ -685,27 +737,53 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - set_wp_with_index(tell_command, packet.seq); + if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission + guided_WP = tell_command; - // update waypoint receiving state machine - waypoint_timelast_receive = millis(); - waypoint_request_i++; + // add home alt if needed + if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ + guided_WP.alt += home.alt; + } - if (waypoint_request_i > g.waypoint_total) - { - uint8_t type = 0; // ok (0), error(1) + set_mode(GUIDED); - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - type); + // make any new wp uploaded instant (in case we are already in Guided mode) + set_guided_WP(); - send_text_P(SEVERITY_LOW,PSTR("flight plan received")); - waypoint_receiving = false; - // XXX ignores waypoint radius for individual waypoints, can - // only set WP_RADIUS parameter - } + // verify we recevied the command + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + 0); + + } else { + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) break; + + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) break; + set_wp_with_index(tell_command, packet.seq); + + // update waypoint receiving state machine + waypoint_timelast_receive = millis(); + waypoint_request_i++; + + if (waypoint_request_i > (uint16_t)g.waypoint_total){ + uint8_t type = 0; // ok (0), error(1) + + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + type); + + send_text_P(SEVERITY_LOW,PSTR("flight plan received")); + waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } + } break; } @@ -717,7 +795,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_param_set_t packet; mavlink_msg_param_set_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; // set parameter @@ -734,7 +814,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // add a small amount before casting parameter values // from float to integer to avoid truncating to the // next lower integer value. - const float rounding_addition = 0.01; + float rounding_addition = 0.01; // fetch the variable type ID var_type = vp->meta_type_id(); @@ -747,12 +827,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ((AP_Float16 *)vp)->set_and_save(packet.param_value); } else if (var_type == AP_Var::k_typeid_int32) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else if (var_type == AP_Var::k_typeid_int16) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); - } else if (var_type == AP_Var::k_typeid_int8) { + if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); } else { // we don't support mavlink set on this parameter @@ -783,8 +864,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - + + if (mavlink_check_target(packet.target_system,packet.target_component)) + break; + v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; @@ -797,19 +880,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) rc_override_fs_timer = millis(); break; } - + case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if(msg->sysid != g.sysid_my_gcs) break; rc_override_fs_timer = millis(); + pmTest1++; break; } -#if HIL_MODE != HIL_MODE_DISABLED + #if HIL_MODE != HIL_MODE_DISABLED // This is used both as a sensor and to pass the location // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: + case MAVLINK_MSG_ID_GPS_RAW: { // decode mavlink_gps_raw_t packet; @@ -951,7 +1035,7 @@ void GCS_MAVLINK::_queued_send() { // Check to see if we are sending parameters - if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) { + if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) { AP_Var *vp; float value; @@ -984,9 +1068,10 @@ GCS_MAVLINK::_queued_send() // request waypoints one by one // XXX note that this is pan-interface if (waypoint_receiving && - (requested_interface == chan) && - waypoint_request_i <= g.waypoint_total && + (requested_interface == (unsigned)chan) && + waypoint_request_i <= (unsigned)g.waypoint_total && mavdelay > 15) { // limits to 3.33 hz + mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, @@ -997,5 +1082,66 @@ GCS_MAVLINK::_queued_send() } } -#endif +#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK +static void send_rate(uint16_t low, uint16_t high) { +#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK + gcs.data_stream_send(low, high); +#endif +#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) + hil.data_stream_send(low,high); +#endif +} + +/* + a delay() callback that processes MAVLink packets. We set this as the + callback in long running library initialisation routines to allow + MAVLink to process packets while waiting for the initialisation to + complete +*/ +static void mavlink_delay(unsigned long t) +{ + unsigned long tstart; + static unsigned long last_1hz, last_3hz, last_10hz, last_50hz; + + if (in_mavlink_delay) { + // this should never happen, but let's not tempt fate by + // letting the stack grow too much + delay(t); + return; + } + + in_mavlink_delay = true; + + tstart = millis(); + do { + unsigned long tnow = millis(); + if (tnow - last_1hz > 1000) { + last_1hz = tnow; + gcs.send_message(MSG_HEARTBEAT); +#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) + hil.send_message(MSG_HEARTBEAT); +#endif + send_rate(1, 3); + } + if (tnow - last_3hz > 333) { + last_3hz = tnow; + send_rate(3, 5); + } + if (tnow - last_10hz > 100) { + last_10hz = tnow; + send_rate(5, 45); + } + if (tnow - last_50hz > 20) { + last_50hz = tnow; + gcs.update(); +#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) + hil.update(); +#endif + send_rate(45, 1000); + } + delay(1); + } while (millis() - tstart < t); + + in_mavlink_delay = false; +} diff --git a/ArduPlane/HIL.h b/ArduPlane/HIL.h index 85b65fa1be..70ad73d6de 100644 --- a/ArduPlane/HIL.h +++ b/ArduPlane/HIL.h @@ -8,7 +8,7 @@ # if HIL_MODE != HIL_MODE_DISABLED -#include +#include #include #include #include @@ -41,7 +41,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(BetterStream *port) { _port = port; } + void init(FastSerial *port) { _port = port; } /// Update HIL state. /// @@ -83,7 +83,7 @@ public: protected: /// The stream we are communicating over - BetterStream *_port; + FastSerial *_port; }; // @@ -111,7 +111,7 @@ class HIL_XPLANE : public HIL_Class public: HIL_XPLANE(); void update(void); - void init(BetterStream *port); + void init(FastSerial *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const prog_char_t *str); diff --git a/ArduPlane/HIL_Xplane.pde b/ArduPlane/HIL_Xplane.pde index 89721b6158..4f01c19028 100644 --- a/ArduPlane/HIL_Xplane.pde +++ b/ArduPlane/HIL_Xplane.pde @@ -55,7 +55,7 @@ HIL_XPLANE::HIL_XPLANE() : } void -HIL_XPLANE::init(BetterStream * port) +HIL_XPLANE::init(FastSerial * port) { HIL_Class::init(port); hilPacketDecoder = new AP_GPS_IMU(port); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 0157bd1727..59a9c66b0b 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#if LOGGING_ENABLED == ENABLED + // Code to Write and Read packets from DataFlash log memory // Code to interact with the user to dump or erase logs @@ -10,7 +12,6 @@ // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. -static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv); static int8_t dump_log(uint8_t argc, const Menu::arg *argv); static int8_t erase_logs(uint8_t argc, const Menu::arg *argv); static int8_t select_logs(uint8_t argc, const Menu::arg *argv); @@ -27,13 +28,14 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv) " enable |all enable logging or everything\n" " disable |all disable logging or everything\n" "\n")); + return 0; } // Creates a constant array of structs representing menu options // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Coommon for implementation details -const struct Menu::command log_menu_commands[] PROGMEM = { +static const struct Menu::command log_menu_commands[] PROGMEM = { {"dump", dump_log}, {"erase", erase_logs}, {"enable", select_logs}, @@ -44,6 +46,8 @@ const struct Menu::command log_menu_commands[] PROGMEM = { // A Macro to create the Menu MENU2(log_menu, "Log", log_menu_commands, print_log_menu); +static void get_log_boundaries(byte log_num, int & start_page, int & end_page); + static bool print_log_menu(void) { @@ -80,7 +84,7 @@ print_log_menu(void) Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); for(int i=1;i 0) { for(int i=0;i 7){ logvar = DataFlash.ReadInt(); }else{ @@ -550,7 +559,7 @@ void Log_Read_Performance() } // Read a command processing packet -void Log_Read_Cmd() +static void Log_Read_Cmd() { byte logvarb; long logvarl; @@ -569,7 +578,7 @@ void Log_Read_Cmd() Serial.println(" "); } -void Log_Read_Startup() +static void Log_Read_Startup() { byte logbyte = DataFlash.ReadByte(); @@ -584,7 +593,7 @@ void Log_Read_Startup() } // Read an attitude packet -void Log_Read_Attitude() +static void Log_Read_Attitude() { Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), DataFlash.ReadInt(), @@ -593,21 +602,22 @@ void Log_Read_Attitude() } // Read a mode packet -void Log_Read_Mode() +static void Log_Read_Mode() { Serial.printf_P(PSTR("MOD:")); Serial.println(flight_mode_strings[DataFlash.ReadByte()]); } // Read a GPS packet -void Log_Read_GPS() +static void Log_Read_GPS() { - Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"), + Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"), DataFlash.ReadLong(), (int)DataFlash.ReadByte(), (int)DataFlash.ReadByte(), (float)DataFlash.ReadLong() / t7, (float)DataFlash.ReadLong() / t7, + (float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing (float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0, @@ -616,7 +626,7 @@ void Log_Read_GPS() } // Read a raw accel/gyro packet -void Log_Read_Raw() +static void Log_Read_Raw() { float logvar; Serial.printf_P(PSTR("RAW:")); @@ -629,13 +639,19 @@ void Log_Read_Raw() } // Read the DataFlash log memory : Packet Parser -void Log_Read(int start_page, int end_page) +static void Log_Read(int start_page, int end_page) { byte data; byte log_step = 0; int packet_count = 0; int page = start_page; + #ifdef AIRFRAME_NAME + Serial.printf_P(PSTR((AIRFRAME_NAME) + #endif + Serial.printf_P(PSTR("\n" THISFIRMWARE + "\nFree RAM: %lu\n"), + freeRAM()); DataFlash.StartRead(start_page); while (page < end_page && page != -1){ @@ -712,4 +728,23 @@ void Log_Read(int start_page, int end_page) Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); } +#else // LOGGING_ENABLED +// dummy functions +static void Log_Write_Mode(byte mode) {} +static void Log_Write_Startup(byte type) {} +static void Log_Write_Cmd(byte num, struct Location *wp) {} +static void Log_Write_Current() {} +static void Log_Write_Nav_Tuning() {} +static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, + long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {} +static void Log_Write_Performance() {} +static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } +static byte get_num_logs(void) { return 0; } +static void start_new_log(byte num_existing_logs) {} +static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {} +static void Log_Write_Control_Tuning() {} +static void Log_Write_Raw() {} + + +#endif // LOGGING_ENABLED diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 6c701c47df..3de4438cc2 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -25,21 +25,36 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } - -void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) +// try to send a message, return false if it won't fit in the serial tx buffer +static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) { uint64_t timeStamp = micros(); + int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; + +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // defer any messages on the telemetry port for 1 second after + // bootup, to try to prevent bricking of Xbees + return false; + } + switch(id) { case MSG_HEARTBEAT: + { + CHECK_PAYLOAD_SIZE(HEARTBEAT); mavlink_msg_heartbeat_send( chan, mavlink_system.type, MAV_AUTOPILOT_ARDUPILOTMEGA); break; + } - case MSG_EXTENDED_STATUS: + case MSG_EXTENDED_STATUS1: { + CHECK_PAYLOAD_SIZE(SYS_STATUS); + uint8_t mode = MAV_MODE_UNINIT; uint8_t nav_mode = MAV_NAV_VECTOR; @@ -52,9 +67,14 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui break; case FLY_BY_WIRE_A: mode = MAV_MODE_TEST2; + nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. break; case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST3; + mode = MAV_MODE_TEST2; + nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case GUIDED: + mode = MAV_MODE_GUIDED; break; case AUTO: mode = MAV_MODE_AUTO; @@ -68,6 +88,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui mode = MAV_MODE_AUTO; nav_mode = MAV_NAV_LOITER; break; + case INITIALISING: + mode = MAV_MODE_UNINIT; + nav_mode = MAV_NAV_GROUNDED; + break; } uint8_t status = MAV_STATE_ACTIVE; @@ -82,11 +106,20 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui battery_voltage * 1000, battery_remaining, packet_drops); - break; + break; } + case MSG_EXTENDED_STATUS2: + { + CHECK_PAYLOAD_SIZE(MEMINFO); + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); + break; + } + case MSG_ATTITUDE: { + CHECK_PAYLOAD_SIZE(ATTITUDE); Vector3f omega = dcm.get_gyro(); mavlink_msg_attitude_send( chan, @@ -102,6 +135,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_LOCATION: { + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_global_position_int_send( chan, @@ -117,6 +151,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_NAV_CONTROLLER_OUTPUT: { if (control_mode != MANUAL) { + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); mavlink_msg_nav_controller_output_send( chan, nav_roll / 1.0e2, @@ -133,6 +168,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_LOCAL_LOCATION: { + CHECK_PAYLOAD_SIZE(LOCAL_POSITION); Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_local_position_send( chan, @@ -148,6 +184,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_GPS_RAW: { + CHECK_PAYLOAD_SIZE(GPS_RAW); mavlink_msg_gps_raw_send( chan, timeStamp, @@ -164,6 +201,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_SERVO_OUT: { + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); uint8_t rssi = 1; // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with HIL maintainers @@ -183,6 +221,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_RADIO_IN: { + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); uint8_t rssi = 1; mavlink_msg_rc_channels_raw_send( chan, @@ -200,6 +239,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_RADIO_OUT: { + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); mavlink_msg_servo_output_raw_send( chan, g.channel_roll.radio_out, @@ -215,6 +255,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_VFR_HUD: { + CHECK_PAYLOAD_SIZE(VFR_HUD); mavlink_msg_vfr_hud_send( chan, (float)airspeed / 100.0, @@ -227,10 +268,12 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui } #if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU: + case MSG_RAW_IMU1: { + CHECK_PAYLOAD_SIZE(RAW_IMU); Vector3f accel = imu.get_accel(); Vector3f gyro = imu.get_gyro(); + //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); mavlink_msg_raw_imu_send( @@ -245,21 +288,42 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui compass.mag_x, compass.mag_y, compass.mag_z); + break; + } - /* This message is not working. Why? + case MSG_RAW_IMU2: + { + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); mavlink_msg_scaled_pressure_send( - chan, - timeStamp, - (float)barometer.Press/100., - (float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw - (int)(barometer.Temp*100)); - */ + chan, + timeStamp, + (float)barometer.Press/100.0, + (float)(barometer.Press-g.ground_pressure)/100.0, + (int)(barometer.Temp*10)); + break; + } + + case MSG_RAW_IMU3: + { + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + Vector3f mag_offsets = compass.get_offsets(); + + mavlink_msg_sensor_offsets_send(chan, + mag_offsets.x, + mag_offsets.y, + mag_offsets.z, + compass.get_declination(), + barometer.RawPress, + barometer.RawTemp, + imu.gx(), imu.gy(), imu.gz(), + imu.ax(), imu.ay(), imu.az()); break; } #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE case MSG_GPS_STATUS: { + CHECK_PAYLOAD_SIZE(GPS_STATUS); mavlink_msg_gps_status_send( chan, g_gps->num_sats, @@ -273,6 +337,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_CURRENT_WAYPOINT: { + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); mavlink_msg_waypoint_current_send( chan, g.waypoint_index); @@ -282,10 +347,77 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui default: break; } + return true; +} + + +#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of + // switch types in mavlink_try_send_message() +static struct mavlink_queue { + uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; + uint8_t next_deferred_message; + uint8_t num_deferred_messages; +} mavlink_queue[2]; + +// send a message using mavlink +static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) +{ + uint8_t i, nextid; + struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; + + // see if we can send the deferred messages, if any + while (q->num_deferred_messages != 0) { + if (!mavlink_try_send_message(chan, + q->deferred_messages[q->next_deferred_message], + packet_drops)) { + break; + } + q->next_deferred_message++; + if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { + q->next_deferred_message = 0; + } + q->num_deferred_messages--; + } + + if (id == MSG_RETRY_DEFERRED) { + return; + } + + // this message id might already be deferred + for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { + if (q->deferred_messages[nextid] == id) { + // its already deferred, discard + return; + } + nextid++; + if (nextid == MAX_DEFERRED_MESSAGES) { + nextid = 0; + } + } + + if (q->num_deferred_messages != 0 || + !mavlink_try_send_message(chan, id, packet_drops)) { + // can't send it now, so defer it + if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { + // the defer buffer is full, discard + return; + } + nextid = q->next_deferred_message + q->num_deferred_messages; + if (nextid >= MAX_DEFERRED_MESSAGES) { + nextid -= MAX_DEFERRED_MESSAGES; + } + q->deferred_messages[nextid] = id; + q->num_deferred_messages++; + } } void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str) { + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // don't send status MAVLink messages for 1 second after + // bootup, to try to prevent Xbee bricking + return; + } mavlink_msg_statustext_send( chan, severity, diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 8d8001b124..f0d930b1fb 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -1,381 +1,468 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef PARAMETERS_H -#define PARAMETERS_H - -#include - -// Global parameter class. -// -class Parameters { -public: - // The version of the layout as described by the parameter enum. - // - // When changing the parameter enum in an incompatible fashion, this - // value should be incremented by one. - // - // The increment will prevent old parameters from being used incorrectly - // by newer code. - // - static const uint16_t k_format_version = 6; - - // - // Parameter identities. - // - // The enumeration defined here is used to ensure that every parameter - // or parameter group has a unique ID number. This number is used by - // AP_Var to store and locate parameters in EEPROM. - // - // Note that entries without a number are assigned the next number after - // the entry preceding them. When adding new entries, ensure that they - // don't overlap. - // - // Try to group related variables together, and assign them a set - // range in the enumeration. Place these groups in numerical order - // at the end of the enumeration. - // - // WARNING: Care should be taken when editing this enumeration as the - // AP_Var load/save code depends on the values here to identify - // variables saved in EEPROM. - // - // - enum { - // Layout version number, always key zero. - // - k_param_format_version = 0, - - - // Misc - // - - k_param_auto_trim, - k_param_switch_enable, - k_param_log_bitmask, - k_param_pitch_trim, - - // 110: Telemetry control - // - k_param_streamrates_port0 = 110, - k_param_streamrates_port3, - k_param_sysid_this_mav, - k_param_sysid_my_gcs, - - // 120: Fly-by-wire control - // - k_param_flybywire_airspeed_min = 120, - k_param_flybywire_airspeed_max, - - // - // 140: Sensor parameters - // - k_param_IMU_calibration = 140, - k_param_altitude_mix, - k_param_airspeed_ratio, - k_param_ground_temperature, - k_param_ground_pressure, - k_param_compass_enabled, - k_param_compass, - k_param_battery_monitoring, - k_param_pack_capacity, - - // - // 160: Navigation parameters - // - k_param_crosstrack_gain = 160, - k_param_crosstrack_entry_angle, - k_param_roll_limit, - k_param_pitch_limit_max, - k_param_pitch_limit_min, - k_param_airspeed_cruise, - k_param_RTL_altitude, - - // - // 180: Radio settings - // - k_param_channel_roll = 180, - k_param_channel_pitch, - k_param_channel_throttle, - k_param_channel_yaw, - k_param_rc_5, - k_param_rc_6, - k_param_rc_7, - k_param_rc_8, - k_param_rc_9, - k_param_rc_10, - - k_param_throttle_min, - k_param_throttle_max, - k_param_throttle_fs_enabled, - k_param_throttle_fs_value, - k_param_throttle_cruise, - k_param_flight_mode_channel, - k_param_flight_modes, - - k_param_short_fs_action, - k_param_long_fs_action, - k_param_gcs_heartbeat_fs_enabled, - - // - // 200: Feed-forward gains - // - k_param_kff_pitch_compensation = 200, - k_param_kff_rudder_mix, - k_param_kff_pitch_to_throttle, - k_param_kff_throttle_to_pitch, - - // - // 220: Waypoint data - // - k_param_waypoint_mode = 220, - k_param_waypoint_total, - k_param_waypoint_index, - k_param_waypoint_radius, - k_param_loiter_radius, - - // - // 240: PID Controllers - // - // Heading-to-roll PID: - // heading error from commnd to roll command deviation from trim - // (bank to turn strategy) - // - k_param_heading_to_roll_PID = 240, - - // Roll-to-servo PID: - // roll error from command to roll servo deviation from trim - // (tracks commanded bank angle) - // - k_param_roll_to_servo_PID, - - // - // Pitch control - // - // Pitch-to-servo PID: - // pitch error from command to pitch servo deviation from trim - // (front-side strategy) - // - k_param_pitch_to_servo_PID, - - // Airspeed-to-pitch PID: - // airspeed error from commnd to pitch servo deviation from trim - // (back-side strategy) - // - k_param_airspeed_to_pitch_PID, - - // - // Yaw control - // - // Yaw-to-servo PID: - // yaw rate error from commnd to yaw servo deviation from trim - // (stabilizes dutch roll) - // - k_param_yaw_to_servo_PID, - - // - // Throttle control - // - // Energy-to-throttle PID: - // total energy error from command to throttle servo deviation from trim - // (throttle back-side strategy alternative) - // - k_param_energy_to_throttle_PID, - - // Altitude-to-pitch PID: - // altitude error from commnd to pitch servo deviation from trim - // (throttle front-side strategy alternative) - // - k_param_altitude_to_pitch_PID, - - // 255: reserved - }; - - AP_Int16 format_version; - - // Telemetry control - // - AP_Int16 sysid_this_mav; - AP_Int16 sysid_my_gcs; - - // Feed-forward gains - // - AP_Float kff_pitch_compensation; - AP_Float kff_rudder_mix; - AP_Float kff_pitch_to_throttle; - AP_Float kff_throttle_to_pitch; - - // Crosstrack navigation - // - AP_Float crosstrack_gain; - AP_Int16 crosstrack_entry_angle; - - // Estimation - // - AP_Float altitude_mix; - AP_Float airspeed_ratio; - - // Waypoints - // - AP_Int8 waypoint_mode; - AP_Int8 waypoint_total; - AP_Int8 waypoint_index; - AP_Int8 waypoint_radius; - AP_Int8 loiter_radius; - - // Fly-by-wire - // - AP_Int8 flybywire_airspeed_min; - AP_Int8 flybywire_airspeed_max; - - // Throttle - // - AP_Int8 throttle_min; - AP_Int8 throttle_max; - AP_Int8 throttle_fs_enabled; - AP_Int16 throttle_fs_value; - AP_Int8 throttle_cruise; - - // Failsafe - AP_Int8 short_fs_action; - AP_Int8 long_fs_action; - AP_Int8 gcs_heartbeat_fs_enabled; - - // Flight modes - // - AP_Int8 flight_mode_channel; - AP_VarA flight_modes; - - // Navigational maneuvering limits - // - AP_Int16 roll_limit; - AP_Int16 pitch_limit_max; - AP_Int16 pitch_limit_min; - - // Misc - // - AP_Int8 auto_trim; - AP_Int8 switch_enable; - AP_Int16 log_bitmask; - AP_Int16 airspeed_cruise; - AP_Int16 pitch_trim; - AP_Int16 RTL_altitude; - AP_Int16 ground_temperature; - AP_Int32 ground_pressure; - AP_Int8 compass_enabled; - AP_Int16 angle_of_attack; - AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current - AP_Int16 pack_capacity; // Battery pack capacity less reserve - - // RC channels - RC_Channel channel_roll; - RC_Channel channel_pitch; - RC_Channel channel_throttle; - RC_Channel channel_rudder; - RC_Channel rc_5; - RC_Channel rc_6; - RC_Channel rc_7; - RC_Channel rc_8; - RC_Channel rc_camera_pitch; - RC_Channel rc_camera_roll; - - // PID controllers - // - PID pidNavRoll; - PID pidServoRoll; - PID pidServoPitch; - PID pidNavPitchAirspeed; - PID pidServoRudder; - PID pidTeThrottle; - PID pidNavPitchAltitude; - - uint8_t junk; - - // Note: keep initializers here in the same order as they are declared above. - Parameters() : - // variable default key name - //------------------------------------------------------------------------------------------------------- - format_version (k_format_version, k_param_format_version, NULL), - - sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), - sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), - - kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")), - kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")), - kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")), - kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")), - - crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), - crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), - - altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")), - airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")), - - /* XXX waypoint_mode missing here */ - waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), - waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), - waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), - loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), - - flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")), - flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")), - - throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), - throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), - throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), - throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), - throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), - - short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), - long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), - gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), - - flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")), - flight_modes (k_param_flight_modes, PSTR("FLTMODE")), - - roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")), - pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")), - pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")), - - auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")), - switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")), - log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")), - airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), - pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), - RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), - ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), - ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), - compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), - battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), - pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), - - // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! - - // RC channel group key name - //---------------------------------------------------------------------- - channel_roll (k_param_channel_roll, PSTR("RC1_")), - channel_pitch (k_param_channel_pitch, PSTR("RC2_")), - channel_throttle (k_param_channel_throttle, PSTR("RC3_")), - channel_rudder (k_param_channel_yaw, PSTR("RC4_")), - rc_5 (k_param_rc_5, PSTR("RC5_")), - rc_6 (k_param_rc_6, PSTR("RC6_")), - rc_7 (k_param_rc_7, PSTR("RC7_")), - rc_8 (k_param_rc_8, PSTR("RC8_")), - rc_camera_pitch (k_param_rc_9, NULL), - rc_camera_roll (k_param_rc_10, NULL), - - // PID controller group key name initial P initial I initial D initial imax - //--------------------------------------------------------------------------------------------------------------------------------------- - pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), - pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), - pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), - pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSPD2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), - pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), - pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), - pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), - - - junk(0) // XXX just so that we can add things without worrying about the trailing comma - { - } -}; - +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#include + +// Global parameter class. +// +class Parameters { +public: + // The version of the layout as described by the parameter enum. + // + // When changing the parameter enum in an incompatible fashion, this + // value should be incremented by one. + // + // The increment will prevent old parameters from being used incorrectly + // by newer code. + // + static const uint16_t k_format_version = 11; + + // The parameter software_type is set up solely for ground station use + // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) + // GCS will interpret values 0-9 as ArduPilotMega. Developers may use + // values within that range to identify different branches. + // + static const uint16_t k_software_type = 0; // 0 for APM trunk + + // + // Parameter identities. + // + // The enumeration defined here is used to ensure that every parameter + // or parameter group has a unique ID number. This number is used by + // AP_Var to store and locate parameters in EEPROM. + // + // Note that entries without a number are assigned the next number after + // the entry preceding them. When adding new entries, ensure that they + // don't overlap. + // + // Try to group related variables together, and assign them a set + // range in the enumeration. Place these groups in numerical order + // at the end of the enumeration. + // + // WARNING: Care should be taken when editing this enumeration as the + // AP_Var load/save code depends on the values here to identify + // variables saved in EEPROM. + // + // + enum { + // Layout version number, always key zero. + // + k_param_format_version = 0, + k_param_software_type, + + + // Misc + // + + k_param_auto_trim, + k_param_switch_enable, + k_param_log_bitmask, + k_param_pitch_trim, + k_param_mix_mode, + k_param_reverse_elevons, + k_param_reverse_ch1_elevon, + k_param_reverse_ch2_elevon, + k_param_flap_1_percent, + k_param_flap_1_speed, + k_param_flap_2_percent, + k_param_flap_2_speed, + k_param_num_resets, + + + // 110: Telemetry control + // + k_param_streamrates_port0 = 110, + k_param_streamrates_port3, + k_param_sysid_this_mav, + k_param_sysid_my_gcs, + k_param_serial3_baud, + + // 120: Fly-by-wire control + // + k_param_flybywire_airspeed_min = 120, + k_param_flybywire_airspeed_max, + + // + // 130: Sensor parameters + // + k_param_IMU_calibration = 130, + k_param_altitude_mix, + k_param_airspeed_ratio, + k_param_ground_temperature, + k_param_ground_pressure, + k_param_compass_enabled, + k_param_compass, + k_param_battery_monitoring, + k_param_pack_capacity, + k_param_airspeed_offset, + k_param_sonar_enabled, + k_param_airspeed_enabled, + + // + // 150: Navigation parameters + // + k_param_crosstrack_gain = 150, + k_param_crosstrack_entry_angle, + k_param_roll_limit, + k_param_pitch_limit_max, + k_param_pitch_limit_min, + k_param_airspeed_cruise, + k_param_RTL_altitude, + k_param_inverted_flight_ch, + + // + // Camera parameters + // +#if CAMERA == ENABLED + k_param_camera, +#endif + + // + // 170: Radio settings + // + k_param_channel_roll = 170, + k_param_channel_pitch, + k_param_channel_throttle, + k_param_channel_yaw, + k_param_rc_5, + k_param_rc_6, + k_param_rc_7, + k_param_rc_8, + + k_param_throttle_min, + k_param_throttle_max, + k_param_throttle_fs_enabled, + k_param_throttle_fs_value, + k_param_throttle_cruise, + + k_param_short_fs_action, + k_param_long_fs_action, + k_param_gcs_heartbeat_fs_enabled, + k_param_throttle_slewrate, + + // + // 200: Feed-forward gains + // + k_param_kff_pitch_compensation = 200, + k_param_kff_rudder_mix, + k_param_kff_pitch_to_throttle, + k_param_kff_throttle_to_pitch, + + // + // 210: flight modes + // + k_param_flight_mode_channel = 210, + k_param_flight_mode1, + k_param_flight_mode2, + k_param_flight_mode3, + k_param_flight_mode4, + k_param_flight_mode5, + k_param_flight_mode6, + + // + // 220: Waypoint data + // + k_param_waypoint_mode = 220, + k_param_waypoint_total, + k_param_waypoint_index, + k_param_waypoint_radius, + k_param_loiter_radius, + + // + // 240: PID Controllers + // + // Heading-to-roll PID: + // heading error from command to roll command deviation from trim + // (bank to turn strategy) + // + k_param_heading_to_roll_PID = 240, + + // Roll-to-servo PID: + // roll error from command to roll servo deviation from trim + // (tracks commanded bank angle) + // + k_param_roll_to_servo_PID, + + // + // Pitch control + // + // Pitch-to-servo PID: + // pitch error from command to pitch servo deviation from trim + // (front-side strategy) + // + k_param_pitch_to_servo_PID, + + // Airspeed-to-pitch PID: + // airspeed error from command to pitch servo deviation from trim + // (back-side strategy) + // + k_param_airspeed_to_pitch_PID, + + // + // Yaw control + // + // Yaw-to-servo PID: + // yaw rate error from command to yaw servo deviation from trim + // (stabilizes dutch roll) + // + k_param_yaw_to_servo_PID, + + // + // Throttle control + // + // Energy-to-throttle PID: + // total energy error from command to throttle servo deviation from trim + // (throttle back-side strategy alternative) + // + k_param_energy_to_throttle_PID, + + // Altitude-to-pitch PID: + // altitude error from command to pitch servo deviation from trim + // (throttle front-side strategy alternative) + // + k_param_altitude_to_pitch_PID, + + // 254,255: reserved + }; + + AP_Int16 format_version; + AP_Int8 software_type; + + // Telemetry control + // + AP_Int16 sysid_this_mav; + AP_Int16 sysid_my_gcs; + AP_Int8 serial3_baud; + + // Feed-forward gains + // + AP_Float kff_pitch_compensation; + AP_Float kff_rudder_mix; + AP_Float kff_pitch_to_throttle; + AP_Float kff_throttle_to_pitch; + + // Crosstrack navigation + // + AP_Float crosstrack_gain; + AP_Int16 crosstrack_entry_angle; + + // Estimation + // + AP_Float altitude_mix; + AP_Float airspeed_ratio; + AP_Int16 airspeed_offset; + + // Waypoints + // + AP_Int8 waypoint_mode; + AP_Int8 waypoint_total; + AP_Int8 waypoint_index; + AP_Int8 waypoint_radius; + AP_Int8 loiter_radius; + + // Fly-by-wire + // + AP_Int8 flybywire_airspeed_min; + AP_Int8 flybywire_airspeed_max; + + // Throttle + // + AP_Int8 throttle_min; + AP_Int8 throttle_max; + AP_Int8 throttle_slewrate; + AP_Int8 throttle_fs_enabled; + AP_Int16 throttle_fs_value; + AP_Int8 throttle_cruise; + + // Failsafe + AP_Int8 short_fs_action; + AP_Int8 long_fs_action; + AP_Int8 gcs_heartbeat_fs_enabled; + + // Flight modes + // + AP_Int8 flight_mode_channel; + AP_Int8 flight_mode1; + AP_Int8 flight_mode2; + AP_Int8 flight_mode3; + AP_Int8 flight_mode4; + AP_Int8 flight_mode5; + AP_Int8 flight_mode6; + + // Navigational maneuvering limits + // + AP_Int16 roll_limit; + AP_Int16 pitch_limit_max; + AP_Int16 pitch_limit_min; + + // Misc + // + AP_Int8 auto_trim; + AP_Int8 switch_enable; + AP_Int8 mix_mode; + AP_Int8 reverse_elevons; + AP_Int8 reverse_ch1_elevon; + AP_Int8 reverse_ch2_elevon; + AP_Int16 num_resets; + AP_Int16 log_bitmask; + AP_Int16 airspeed_cruise; + AP_Int16 pitch_trim; + AP_Int16 RTL_altitude; + AP_Int16 ground_temperature; + AP_Int32 ground_pressure; + AP_Int8 compass_enabled; + AP_Int16 angle_of_attack; + AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current + AP_Int16 pack_capacity; // Battery pack capacity less reserve + AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger + AP_Int8 sonar_enabled; + AP_Int8 airspeed_enabled; + AP_Int8 flap_1_percent; + AP_Int8 flap_1_speed; + AP_Int8 flap_2_percent; + AP_Int8 flap_2_speed; + + // Camera +#if CAMERA == ENABLED + AP_Camera camera; +#endif + + // RC channels + RC_Channel channel_roll; + RC_Channel channel_pitch; + RC_Channel channel_throttle; + RC_Channel channel_rudder; + RC_Channel_aux rc_5; + RC_Channel_aux rc_6; + RC_Channel_aux rc_7; + RC_Channel_aux rc_8; + + // PID controllers + // + PID pidNavRoll; + PID pidServoRoll; + PID pidServoPitch; + PID pidNavPitchAirspeed; + PID pidServoRudder; + PID pidTeThrottle; + PID pidNavPitchAltitude; + + uint8_t junk; + + // Note: keep initializers here in the same order as they are declared above. + Parameters() : + // variable default key name + //------------------------------------------------------------------------------------------------------- + format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")), + software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")), + + sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), + sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), + serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")), + + kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")), + kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")), + kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")), + kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")), + + crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), + crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), + + altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")), + airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")), + airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")), + + /* XXX waypoint_mode missing here */ + waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), + waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), + loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), + + flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")), + flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")), + + throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), + throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), + throttle_slewrate (THROTTLE_SLEW_LIMIT, k_param_throttle_slewrate, PSTR("THR_SLEWRATE")), + throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), + throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), + throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), + + short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), + long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), + gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), + + flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")), + flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")), + flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")), + flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")), + flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")), + flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")), + flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")), + + roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")), + pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")), + pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")), + + auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")), + switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")), + mix_mode (ELEVON_MIXING, k_param_mix_mode, PSTR("ELEVON_MIXING")), + reverse_elevons (ELEVON_REVERSE, k_param_reverse_elevons, PSTR("ELEVON_REVERSE")), + reverse_ch1_elevon (ELEVON_CH1_REVERSE, k_param_reverse_ch1_elevon, PSTR("ELEVON_CH1_REV")), + reverse_ch2_elevon (ELEVON_CH2_REVERSE, k_param_reverse_ch2_elevon, PSTR("ELEVON_CH2_REV")), + num_resets (0, k_param_num_resets, PSTR("SYS_NUM_RESETS")), + log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), + airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), + pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), + RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), + ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), + ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), + compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), + flap_1_percent (FLAP_1_PERCENT, k_param_flap_1_percent, PSTR("FLAP_1_PERCNT")), + flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")), + flap_2_percent (FLAP_2_PERCENT, k_param_flap_2_percent, PSTR("FLAP_2_PERCNT")), + flap_2_speed (FLAP_2_SPEED, k_param_flap_2_speed, PSTR("FLAP_2_SPEED")), + + + battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), + pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), + inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), + sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")), + airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")), + + // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! + +#if CAMERA == ENABLED + camera (k_param_camera, PSTR("CAM_")), +#endif + + // RC channel group key name + //---------------------------------------------------------------------- + channel_roll (k_param_channel_roll, PSTR("RC1_")), + channel_pitch (k_param_channel_pitch, PSTR("RC2_")), + channel_throttle (k_param_channel_throttle, PSTR("RC3_")), + channel_rudder (k_param_channel_yaw, PSTR("RC4_")), + rc_5 (k_param_rc_5, PSTR("RC5_")), + rc_6 (k_param_rc_6, PSTR("RC6_")), + rc_7 (k_param_rc_7, PSTR("RC7_")), + rc_8 (k_param_rc_8, PSTR("RC8_")), + + // PID controller group key name initial P initial I initial D initial imax + //--------------------------------------------------------------------------------------------------------------------------------------- + pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), + pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), + pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), + pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSP2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), + pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), + pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), + pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), + + + junk(0) // XXX just so that we can add things without worrying about the trailing comma + { + } +}; + #endif // PARAMETERS_H diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde index 733a81a129..d497409342 100644 --- a/ArduPlane/WP_activity.pde +++ b/ArduPlane/WP_activity.pde @@ -1,49 +1,56 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#if CAMERA == ENABLED void waypoint_check() { if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < 10){ // Get as close as it can for you - servo_pic(); + if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you + g.camera.trigger_pic(); } // DO SOMETHIMNG } if(g.waypoint_index == 20){ // When here do whats underneath - servo_pic(); + g.camera.trigger_pic(); } } void picture_time_check() { - if (picture_time == 1){ - if (wp_distance < 10){ - servo_pic(); // or any camera activation command + if (g.camera.picture_time == 1){ + if (wp_distance < g.camera.wp_distance_min){ + g.camera.trigger_pic(); // or any camera activation command } } } +#endif void egg_waypoint() { - float temp = (float)(current_loc.alt - home.alt) * .01; - float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; + if (g_rc_function[RC_Channel_aux::k_egg_drop]) + { + float temp = (float)(current_loc.alt - home.alt) * .01; + float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; - if(g.waypoint_index == 3){ - if(wp_distance < egg_dist){ - APM_RC.OutputCh(CH_RUDDER,1500 + (-45*10.31)); + if(g.waypoint_index == 3){ + if(wp_distance < egg_dist){ + g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (-45*10.31); + } + }else{ + g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (45*10.31); } - }else{ - APM_RC.OutputCh(CH_RUDDER,1500 + (45*10.31)); } } +#if CAMERA == ENABLED void johann_check() // if you aren't Johann it doesn't really matter :D { APM_RC.OutputCh(CH_7,1500 + (350)); if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < 10){ // Get as close as it can for you - servo_pic(); + if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you + g.camera.trigger_pic(); } } if(g.waypoint_index == 20){ // When here do whats underneath - servo_pic(); + g.camera.trigger_pic(); } } +#endif diff --git a/ArduPlane/climb_rate.pde b/ArduPlane/climb_rate.pde index ec1247a85e..b7e6b81d74 100644 --- a/ArduPlane/climb_rate.pde +++ b/ArduPlane/climb_rate.pde @@ -1,10 +1,12 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + struct DataPoint { unsigned long x; long y; }; DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from -unsigned char index; // Index in history for the current data point +unsigned char hindex; // Index in history for the current data point unsigned long xoffset; unsigned char n; @@ -15,12 +17,9 @@ long yi; long xiyi; unsigned long xi2; - -void add_altitude_data(unsigned long xl, long y) +#if 0 // currently unused +static void add_altitude_data(unsigned long xl, long y) { - unsigned char i; - int dx; - //Reset the regression if our X variable overflowed if (xl < xoffset) n = 0; @@ -30,10 +29,10 @@ void add_altitude_data(unsigned long xl, long y) n = 0; if (n == ALTITUDE_HISTORY_LENGTH) { - xi -= history[index].x; - yi -= history[index].y; - xiyi -= (long)history[index].x * history[index].y; - xi2 -= history[index].x * history[index].x; + xi -= history[hindex].x; + yi -= history[hindex].y; + xiyi -= (long)history[hindex].x * history[hindex].y; + xi2 -= history[hindex].x * history[hindex].x; } else { if (n == 0) { xoffset = xl; @@ -45,31 +44,33 @@ void add_altitude_data(unsigned long xl, long y) n++; } - history[index].x = xl - xoffset; - history[index].y = y; + history[hindex].x = xl - xoffset; + history[hindex].y = y; - xi += history[index].x; - yi += history[index].y; - xiyi += (long)history[index].x * history[index].y; - xi2 += history[index].x * history[index].x; + xi += history[hindex].x; + yi += history[hindex].y; + xiyi += (long)history[hindex].x * history[hindex].y; + xi2 += history[hindex].x * history[hindex].x; - if (++index >= ALTITUDE_HISTORY_LENGTH) - index = 0; + if (++hindex >= ALTITUDE_HISTORY_LENGTH) + hindex = 0; } +#endif -void recalc_climb_rate() +#if 0 // unused +static void recalc_climb_rate() { float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2); climb_rate = (int)(slope*100); } -void print_climb_debug_info() +static void print_climb_debug_info() { unsigned char i, j; recalc_climb_rate(); //SendDebugln_P("Climb rate:"); for (i=0; i= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH; //SendDebug_P(" "); //SendDebug(j,DEC); @@ -90,3 +91,4 @@ void print_climb_debug_info() //SendDebug((float)climb_rate/100,2); //SendDebugln_P(" m/s"); } +#endif diff --git a/ArduPlane/command description.txt b/ArduPlane/command description.txt index b2668a8b90..2d3102c9d9 100644 --- a/ArduPlane/command description.txt +++ b/ArduPlane/command description.txt @@ -45,7 +45,7 @@ May Commands - these commands are optional to finish Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) - -0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - - +0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT - alt (finish) rate (cm/sec) - Note: rate must be > 10 cm/sec due to integer math MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 5ccbf7e486..0ad04ee3da 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -1,6 +1,6 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -void init_commands() +static void init_commands() { //read_EEPROM_waypoint_info(); g.waypoint_index.set_and_save(0); @@ -9,15 +9,19 @@ void init_commands() next_command.id = CMD_BLANK; } -void update_auto() +static void update_auto() { - if (g.waypoint_index == g.waypoint_total) { - do_RTL(); + if (g.waypoint_index >= g.waypoint_total) { + handle_no_commands(); + if(g.waypoint_total == 0) { + next_WP.lat = home.lat + 1000; // so we don't have bad calcs + next_WP.lng = home.lng + 1000; // so we don't have bad calcs + } } } // this is only used by an air-start -void reload_commands_airstart() +static void reload_commands_airstart() { init_commands(); g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? @@ -26,7 +30,7 @@ void reload_commands_airstart() // Getters // ------- -struct Location get_wp_with_index(int i) +static struct Location get_wp_with_index(int i) { struct Location temp; long mem; @@ -66,12 +70,13 @@ struct Location get_wp_with_index(int i) // Setters // ------- -void set_wp_with_index(struct Location temp, int i) +static void set_wp_with_index(struct Location temp, int i) { i = constrain(i, 0, g.waypoint_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - - // Set altitude options bitmask + + // Set altitude options bitmask + // XXX What is this trying to do? if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){ temp.options = MASK_OPTIONS_RELATIVE_ALT; } else { @@ -96,26 +101,26 @@ void set_wp_with_index(struct Location temp, int i) eeprom_write_dword((uint32_t *) mem, temp.lng); } -void increment_WP_index() +static void increment_WP_index() { - if (g.waypoint_index < g.waypoint_total) { + if (g.waypoint_index <= g.waypoint_total) { g.waypoint_index.set_and_save(g.waypoint_index + 1); - SendDebug_P("MSG WP index is incremented to "); + SendDebug_P("MSG - WP index is incremented to "); }else{ //SendDebug_P("MSG Failed to increment WP index of "); // This message is used excessively at the end of a mission } - SendDebugln(g.waypoint_index,DEC); + //SendDebugln(g.waypoint_index,DEC); } -void decrement_WP_index() +static void decrement_WP_index() { if (g.waypoint_index > 0) { g.waypoint_index.set_and_save(g.waypoint_index - 1); } } -long read_alt_to_hold() +static long read_alt_to_hold() { if(g.RTL_altitude < 0) return current_loc.alt; @@ -124,30 +129,14 @@ long read_alt_to_hold() } -//******************************************************************************** -//This function sets the waypoint and modes for Return to Launch -//******************************************************************************** - -Location get_LOITER_home_wp() -{ - //so we know where we are navigating from - next_WP = current_loc; - - // read home position - struct Location temp = get_wp_with_index(0); - temp.id = MAV_CMD_NAV_LOITER_UNLIM; - temp.alt = read_alt_to_hold(); - return temp; -} - /* This function stores waypoint commands It looks to see what the next command type is and finds the last command. */ -void set_next_WP(struct Location *wp) +static void set_next_WP(struct Location *wp) { //gcs.send_text_P(SEVERITY_LOW,PSTR("load WP")); - SendDebug_P("MSG wp_index: "); + SendDebug_P("MSG - wp_index: "); SendDebugln(g.waypoint_index, DEC); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -173,6 +162,44 @@ void set_next_WP(struct Location *wp) loiter_sum = 0; loiter_total = 0; + // this is used to offset the shrinking longitude as we go towards the poles + float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; + scaleLongDown = cos(rads); + scaleLongUp = 1.0f/cos(rads); + // this is handy for the groundstation + wp_totalDistance = get_distance(¤t_loc, &next_WP); + wp_distance = wp_totalDistance; + target_bearing = get_bearing(¤t_loc, &next_WP); + nav_bearing = target_bearing; + + // to check if we have missed the WP + // ---------------------------- + old_target_bearing = target_bearing; + + // set a new crosstrack bearing + // ---------------------------- + reset_crosstrack(); + + gcs.print_current_waypoints(); +} + +static void set_guided_WP(void) +{ + SendDebug_P("MSG - set_guided_wp"); + + // copy the current location into the OldWP slot + // --------------------------------------- + prev_WP = current_loc; + + // Load the next_WP slot + // --------------------- + next_WP = guided_WP; + + // used to control FBW and limit the rate of climb + // ----------------------------------------------- + target_altitude = current_loc.alt; + offset_altitude = next_WP.alt - prev_WP.alt; + // this is used to offset the shrinking longitude as we go towards the poles float rads = (abs(next_WP.lat)/t7) * 0.0174532925; scaleLongDown = cos(rads); @@ -190,15 +217,13 @@ void set_next_WP(struct Location *wp) // set a new crosstrack bearing // ---------------------------- reset_crosstrack(); - - gcs.print_current_waypoints(); } // run this at setup on the ground // ------------------------------- void init_home() { - SendDebugln("MSG: init home"); + SendDebugln("MSG: init home"); // block until we get a good fix // ----------------------------- @@ -221,6 +246,12 @@ void init_home() // Save prev loc // ------------- next_WP = prev_WP = home; + + // Load home for a default guided_WP + // ------------- + guided_WP = home; + guided_WP.alt += g.RTL_altitude; + } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 1abbfe3693..7da8327970 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -3,7 +3,7 @@ /********************************************************************************/ // Command Event Handlers /********************************************************************************/ -void +static void handle_process_must() { // reset navigation integrators @@ -39,13 +39,13 @@ handle_process_must() case MAV_CMD_NAV_RETURN_TO_LAUNCH: do_RTL(); break; - + default: break; } } -void +static void handle_process_may() { switch(next_command.id){ @@ -82,7 +82,7 @@ handle_process_may() } } -void handle_process_now() +static void handle_process_now() { switch(next_command.id){ @@ -116,27 +116,18 @@ void handle_process_now() } } -void handle_no_commands() +static void handle_no_commands() { - if (command_must_ID) - return; - - switch (control_mode){ - case LAND: - // don't get a new command - break; - - default: - set_mode(RTL); - break; - } + next_command = home; + next_command.alt = read_alt_to_hold(); + next_command.id = MAV_CMD_NAV_LOITER_UNLIM; } /********************************************************************************/ // Verify command Handlers /********************************************************************************/ -bool verify_must() +static bool verify_must() { switch(command_must_ID) { @@ -175,7 +166,7 @@ bool verify_must() } } -bool verify_may() +static bool verify_may() { switch(command_may_ID) { case NO_COMMAND: @@ -195,18 +186,18 @@ bool verify_may() default: gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); - return false; break; } + return false; } /********************************************************************************/ // Nav (Must) commands /********************************************************************************/ -void do_RTL(void) +static void do_RTL(void) { - control_mode = LOITER; + control_mode = RTL; crash_timer = 0; next_WP = home; @@ -222,7 +213,7 @@ void do_RTL(void) Log_Write_Mode(control_mode); } -void do_takeoff() +static void do_takeoff() { set_next_WP(&next_command); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 @@ -236,28 +227,28 @@ void do_takeoff() takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction } -void do_nav_wp() +static void do_nav_wp() { set_next_WP(&next_command); } -void do_land() +static void do_land() { set_next_WP(&next_command); } -void do_loiter_unlimited() +static void do_loiter_unlimited() { set_next_WP(&next_command); } -void do_loiter_turns() +static void do_loiter_turns() { set_next_WP(&next_command); loiter_total = next_command.p1 * 360; } -void do_loiter_time() +static void do_loiter_time() { set_next_WP(&next_command); loiter_time = millis(); @@ -267,7 +258,7 @@ void do_loiter_time() /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ -bool verify_takeoff() +static bool verify_takeoff() { if (g_gps->ground_speed > 300){ if(hold_course == -1){ @@ -296,7 +287,7 @@ bool verify_takeoff() } } -bool verify_land() +static bool verify_land() { // we don't verify landing - we never go to a new Must command after Land if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) @@ -323,7 +314,7 @@ bool verify_land() return false; } -bool verify_nav_wp() +static bool verify_nav_wp() { hold_course = -1; update_crosstrack(); @@ -344,25 +335,25 @@ bool verify_nav_wp() return false; } -bool verify_loiter_unlim() +static bool verify_loiter_unlim() { update_loiter(); calc_bearing_error(); return false; } -bool verify_loiter_time() +static bool verify_loiter_time() { update_loiter(); calc_bearing_error(); - if ((millis() - loiter_time) > (long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds + if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); return true; } return false; } -bool verify_loiter_turns() +static bool verify_loiter_turns() { update_loiter(); calc_bearing_error(); @@ -375,7 +366,7 @@ bool verify_loiter_turns() return false; } -bool verify_RTL() +static bool verify_RTL() { if (wp_distance <= g.waypoint_radius) { gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home")); @@ -389,22 +380,22 @@ bool verify_RTL() // Condition (May) commands /********************************************************************************/ -void do_wait_delay() +static void do_wait_delay() { condition_start = millis(); condition_value = next_command.lat * 1000; // convert to milliseconds } -void do_change_alt() +static void do_change_alt() { - condition_rate = next_command.p1; + condition_rate = next_command.lat; condition_value = next_command.alt; target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update next_WP.alt = condition_value; // For future nav calculations offset_altitude = 0; // For future nav calculations } -void do_within_distance() +static void do_within_distance() { condition_value = next_command.lat; } @@ -413,23 +404,18 @@ void do_within_distance() // Verify Condition (May) commands /********************************************************************************/ -bool verify_wait_delay() +static bool verify_wait_delay() { - if ((millis() - condition_start) > condition_value){ + if ((unsigned)(millis() - condition_start) > condition_value){ condition_value = 0; return true; } return false; } -bool verify_change_alt() +static bool verify_change_alt() { - //XXX this doesn't look right. How do you descend? - if(current_loc.alt >= condition_value) { - //Serial.printf_P(PSTR("alt, top:")); - //Serial.print(current_loc.alt, DEC); - //Serial.printf_P(PSTR("\t")); - //Serial.println(condition_value, DEC); + if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) { condition_value = 0; return true; } @@ -437,7 +423,7 @@ bool verify_change_alt() return false; } -bool verify_within_distance() +static bool verify_within_distance() { if (wp_distance < condition_value){ condition_value = 0; @@ -450,12 +436,12 @@ bool verify_within_distance() // Do (Now) commands /********************************************************************************/ -void do_loiter_at_location() +static void do_loiter_at_location() { next_WP = current_loc; } -void do_jump() +static void do_jump() { struct Location temp; if(next_command.lat > 0) { @@ -467,20 +453,22 @@ void do_jump() set_wp_with_index(temp, g.waypoint_index); g.waypoint_index.set_and_save(next_command.p1 - 1); + } else if (next_command.lat == -1) { + g.waypoint_index.set_and_save(next_command.p1 - 1); } } -void do_change_speed() +static void do_change_speed() { // Note: we have no implementation for commanded ground speed, only air speed and throttle if(next_command.alt > 0) g.airspeed_cruise.set_and_save(next_command.alt * 100); if(next_command.lat > 0) - g.throttle_cruise.set_and_save(next_command.lat * 100); + g.throttle_cruise.set_and_save(next_command.lat); } -void do_set_home() +static void do_set_home() { if(next_command.p1 == 1 && GPS_enabled) { init_home(); @@ -493,12 +481,12 @@ void do_set_home() } } -void do_set_servo() +static void do_set_servo() { APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); } -void do_set_relay() +static void do_set_relay() { if (next_command.p1 == 1) { relay_on(); @@ -509,7 +497,7 @@ void do_set_relay() } } -void do_repeat_servo() +static void do_repeat_servo() { event_id = next_command.p1 - 1; @@ -538,11 +526,11 @@ void do_repeat_servo() } } -void do_repeat_relay() +static void do_repeat_relay() { event_id = RELAY_TOGGLE; event_timer = 0; event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_repeat = next_command.alt * 2; update_events(); -} +} diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 466ad2589a..c0b2576e92 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -2,15 +2,15 @@ // For changing active command mid-mission //---------------------------------------- -void change_command(uint8_t index) +static void change_command(uint8_t cmd_index) { - struct Location temp = get_wp_with_index(index); + struct Location temp = get_wp_with_index(cmd_index); if (temp.id > MAV_CMD_NAV_LAST ){ gcs.send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { command_must_index = NO_COMMAND; next_command.id = NO_COMMAND; - g.waypoint_index.set_and_save(index - 1); + g.waypoint_index.set_and_save(cmd_index - 1); load_next_command_from_EEPROM(); process_next_command(); } @@ -19,7 +19,7 @@ void change_command(uint8_t index) // called by 10 Hz loop // -------------------- -void update_commands(void) +static void update_commands(void) { // This function loads commands into three buffers // when a new command is loaded, it is processed with process_XXX() @@ -34,7 +34,7 @@ void update_commands(void) } // Other (eg GCS_Auto) modes may be implemented here } -void verify_commands(void) +static void verify_commands(void) { if(verify_must()){ command_must_index = NO_COMMAND; @@ -46,7 +46,7 @@ void verify_commands(void) } } -void load_next_command_from_EEPROM() +static void load_next_command_from_EEPROM() { // fetch next command if the next command queue is empty // ----------------------------------------------------- @@ -64,7 +64,7 @@ void load_next_command_from_EEPROM() } } -void process_next_command() +static void process_next_command() { // these are Navigation/Must commands // --------------------------------- @@ -115,7 +115,7 @@ void process_next_command() /**************************************************/ // These functions implement the commands. /**************************************************/ -void process_must() +static void process_must() { gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -132,7 +132,7 @@ void process_must() next_command.id = NO_COMMAND; } -void process_may() +static void process_may() { gcs.send_text_P(SEVERITY_LOW,PSTR("")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -145,7 +145,7 @@ void process_may() next_command.id = NO_COMMAND; } -void process_now() +static void process_now() { handle_process_now(); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 003c1ea8e3..f5fac05daa 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -46,13 +46,31 @@ // AIRSPEED_SENSOR // AIRSPEED_RATIO // -//#ifndef AIRSPEED_SENSOR -//# define AIRSPEED_SENSOR DISABLED -//#endif +#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 +////////////////////////////////////////////////////////////////////////////// +// Sonar +// + +#ifndef SONAR_ENABLED +# define SONAR_ENABLED DISABLED +#endif + +#ifndef SONAR_PIN +# define SONAR_PIN AN4 // AN5, AP_RANGEFINDER_PITOT_TUBE +#endif + +#ifndef SONAR_TYPE +# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL, +#endif + + ////////////////////////////////////////////////////////////////////////////// // HIL_PROTOCOL OPTIONAL // HIL_MODE OPTIONAL @@ -169,9 +187,6 @@ #ifndef MAG_ORIENTATION # define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #endif -#ifndef MAG_PROTOCOL -# define MAG_PROTOCOL MAG_PROTOCOL_5843 -#endif ////////////////////////////////////////////////////////////////////////////// @@ -184,8 +199,7 @@ ////////////////////////////////////////////////////////////////////////////// // Radio channel limits // -// Note that these are not called out in APM_Config.h.example as there -// is currently no configured behaviour for these channels. +// Note that these are not called out in APM_Config.h.reference. // #ifndef CH5_MIN # define CH5_MIN 1000 @@ -213,6 +227,18 @@ #endif +#ifndef FLAP_1_PERCENT +# define FLAP_1_PERCENT 0 +#endif +#ifndef FLAP_1_SPEED +# define FLAP_1_SPEED 255 +#endif +#ifndef FLAP_2_PERCENT +# define FLAP_2_PERCENT 0 +#endif +#ifndef FLAP_2_SPEED +# define FLAP_2_SPEED 255 +#endif ////////////////////////////////////////////////////////////////////////////// // FLIGHT_MODE // FLIGHT_MODE_CHANNEL @@ -256,14 +282,14 @@ #ifndef THROTTLE_FAILSAFE # define THROTTLE_FAILSAFE ENABLED #endif -#ifndef THROTTE_FS_VALUE +#ifndef THROTTLE_FS_VALUE # define THROTTLE_FS_VALUE 950 #endif #ifndef SHORT_FAILSAFE_ACTION -# define SHORT_FAILSAFE_ACTION 2 +# define SHORT_FAILSAFE_ACTION 0 #endif #ifndef LONG_FAILSAFE_ACTION -# define LONG_FAILSAFE_ACTION 2 +# define LONG_FAILSAFE_ACTION 0 #endif #ifndef GCS_HEARTBEAT_FAILSAFE # define GCS_HEARTBEAT_FAILSAFE DISABLED @@ -330,6 +356,22 @@ # define REVERSE_SWITCH ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// ENABLE ELEVON_MIXING +// +#ifndef ELEVON_MIXING +# define ELEVON_MIXING DISABLED +#endif +#ifndef ELEVON_REVERSE +# define ELEVON_REVERSE DISABLED +#endif +#ifndef ELEVON_CH1_REVERSE +# define ELEVON_CH1_REVERSE DISABLED +#endif +#ifndef ELEVON_CH2_REVERSE +# define ELEVON_CH2_REVERSE DISABLED +#endif + ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// @@ -556,6 +598,12 @@ ////////////////////////////////////////////////////////////////////////////// // Dataflash logging control // + +#ifndef LOGGING_ENABLED +# define LOGGING_ENABLED ENABLED +#endif + + #ifndef LOG_ATTITUDE_FAST # define LOG_ATTITUDE_FAST DISABLED #endif @@ -587,6 +635,22 @@ # define LOG_CUR DISABLED #endif +// calculate the default log_bitmask +#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) + +#define DEFAULT_LOG_BITMASK \ + LOGBIT(ATTITUDE_FAST) | \ + LOGBIT(ATTITUDE_MED) | \ + LOGBIT(GPS) | \ + LOGBIT(PM) | \ + LOGBIT(CTUN) | \ + LOGBIT(NTUN) | \ + LOGBIT(MODE) | \ + LOGBIT(RAW) | \ + LOGBIT(CMD) | \ + LOGBIT(CUR) + + #ifndef DEBUG_PORT # define DEBUG_PORT 0 #endif @@ -633,6 +697,10 @@ # define USE_CURRENT_ALT FALSE #endif +#ifndef INVERTED_FLIGHT_PWM +# define INVERTED_FLIGHT_PWM 1750 +#endif + ////////////////////////////////////////////////////////////////////////////// // Developer Items // @@ -643,3 +711,17 @@ #endif #define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE) +// use this to enable servos in HIL mode +#ifndef HIL_SERVOS +# define HIL_SERVOS DISABLED +#endif + +// use this to completely disable the CLI +#ifndef CLI_ENABLED +# define CLI_ENABLED ENABLED +#endif + +// delay to prevent Xbee bricking, in milliseconds +#ifndef MAVLINK_TELEMETRY_PORT_DELAY +# define MAVLINK_TELEMETRY_PORT_DELAY 2000 +#endif diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index b760195eb9..4c679f2e52 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -1,9 +1,11 @@ -void read_control_switch() +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +static void read_control_switch() { byte switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition){ - set_mode(g.flight_modes[switchPosition]); + set_mode(flight_modes[switchPosition]); oldSwitchPosition = switchPosition; prev_WP = current_loc; @@ -12,9 +14,15 @@ void read_control_switch() // ------------------------- reset_I(); } + + if (g.inverted_flight_ch != 0) { + // if the user has configured an inverted flight channel, then + // fly upside down when that channel goes above INVERTED_FLIGHT_PWM + inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); + } } -byte readSwitch(void){ +static byte readSwitch(void){ uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; @@ -24,7 +32,7 @@ byte readSwitch(void){ return 0; } -void reset_control_switch() +static void reset_control_switch() { oldSwitchPosition = 0; read_control_switch(); @@ -32,52 +40,23 @@ void reset_control_switch() SendDebugln(oldSwitchPosition , DEC); } -void update_servo_switches() +static void update_servo_switches() { + if (!g.switch_enable) { + // switches are disabled in EEPROM (see SWITCH_ENABLE option) + // this means the EEPROM control of all channel reversal is enabled + return; + } // up is reverse // up is elevon - mix_mode = (PINL & 128) ? 1 : 0; - if (mix_mode == 0) { - reverse_roll = (PINE & 128) ? true : false; - reverse_pitch = (PINE & 64) ? true : false; - reverse_rudder = (PINL & 64) ? true : false; + g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode + if (g.mix_mode == 0) { + g.channel_roll.set_reverse((PINE & 128) ? true : false); + g.channel_pitch.set_reverse((PINE & 64) ? true : false); + g.channel_rudder.set_reverse((PINL & 64) ? true : false); } else { - reverse_elevons = (PINE & 128) ? -1 : 1; - reverse_ch1_elevon = (PINE & 64) ? -1 : 1; - reverse_ch2_elevon = (PINL & 64) ? -1 : 1; - } -} - - -bool trim_flag; -unsigned long trim_timer; - -// read at 10 hz -// set this to your trainer switch -void read_trim_switch() -{ - // switch is engaged - if (g.rc_7.control_in > 800){ - if(trim_flag == false){ - // called once - trim_timer = millis(); - } - trim_flag = true; - - }else{ // switch is disengaged - - if(trim_flag){ - // switch was just released - if((millis() - trim_timer) > 2000){ - - g.throttle_cruise.set_and_save(g.channel_throttle.control_in); - g.angle_of_attack.set_and_save(dcm.pitch_sensor); - g.airspeed_cruise.set_and_save(airspeed); - - } else { - - } - trim_flag = false; - } + g.reverse_elevons = (PINE & 128) ? true : false; + g.reverse_ch1_elevon = (PINE & 64) ? true : false; + g.reverse_ch2_elevon = (PINL & 64) ? true : false; } } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 43ac0226d6..fff75fda4f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -10,11 +10,9 @@ #define DEBUG 0 #define LOITER_RANGE 60 // for calculating power outside of loiter radius -#define ROLL_SERVO_MAX 4500 -#define PITCH_SERVO_MAX 4500 -#define RUDDER_SERVO_MAX 4500 +#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. -// failsafe +// failsafe // ---------------------- #define FAILSAFE_NONE 0 #define FAILSAFE_SHORT 1 @@ -32,10 +30,6 @@ #define T6 1000000 #define T7 10000000 -//MAGNETOMETER -#define MAG_PROTOCOL_5843 0 -#define MAG_PROTOCOL_5883L 1 - // GPS type codes - use the names, not the numbers #define GPS_PROTOCOL_NONE -1 #define GPS_PROTOCOL_NMEA 0 @@ -90,12 +84,15 @@ #define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle #define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed - // Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0 +#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed + // Fly By Wire B and Fly By Wire C require airspeed sensor #define AUTO 10 #define RTL 11 #define LOITER 12 -#define TAKEOFF 13 -#define LAND 14 +//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM +//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM +#define GUIDED 15 +#define INITIALISING 16 // in startup routines // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library @@ -118,6 +115,10 @@ #define MAV_CMD_CONDITION_YAW 23 // GCS Message ID's +/// NOTE: to ensure we never block on sending MAVLink messages +/// please keep each MSG_ to a single MAVLink message. If need be +/// create new MSG_ IDs for additional messages on the same +/// stream #define MSG_ACKNOWLEDGE 0x00 #define MSG_HEARTBEAT 0x01 #define MSG_ATTITUDE 0x02 @@ -128,9 +129,10 @@ #define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed #define MSG_VERSION_REQUEST 0x08 #define MSG_VERSION 0x09 -#define MSG_EXTENDED_STATUS 0x0a -#define MSG_CPU_LOAD 0x0b -#define MSG_NAV_CONTROLLER_OUTPUT 0x0c +#define MSG_EXTENDED_STATUS1 0x0a +#define MSG_EXTENDED_STATUS2 0x0b +#define MSG_CPU_LOAD 0x0c +#define MSG_NAV_CONTROLLER_OUTPUT 0x0d #define MSG_COMMAND_REQUEST 0x20 #define MSG_COMMAND_UPLOAD 0x21 @@ -153,9 +155,11 @@ #define MSG_RADIO_OUT 0x53 #define MSG_RADIO_IN 0x54 -#define MSG_RAW_IMU 0x60 -#define MSG_GPS_STATUS 0x61 -#define MSG_GPS_RAW 0x62 +#define MSG_RAW_IMU1 0x60 +#define MSG_RAW_IMU2 0x61 +#define MSG_RAW_IMU3 0x62 +#define MSG_GPS_STATUS 0x63 +#define MSG_GPS_RAW 0x64 #define MSG_SERVO_OUT 0x70 @@ -173,6 +177,7 @@ #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 #define MSG_LOCAL_LOCATION 0xb4 +#define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1 #define SEVERITY_MEDIUM 2 @@ -205,7 +210,6 @@ #define MASK_LOG_RAW (1<<7) #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CUR (1<<9) -#define MASK_LOG_SET_DEFAULTS (1<<15) // Waypoint Modes // ---------------- @@ -246,7 +250,11 @@ // sonar +#define MAX_SONAR_XL 0 +#define MAX_SONAR_LV 1 #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters +#define AN4 4 +#define AN5 5 // Hardware Parameters #define SLIDE_SWITCH_PIN 40 @@ -267,3 +275,6 @@ #define ONBOARD_PARAM_NAME_LENGTH 15 #define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe + +// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1) +#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1) diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index e0445f06d1..acd60e9cfc 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -void failsafe_short_on_event() +static void failsafe_short_on_event() { // This is how to handle a short loss of control signal failsafe. failsafe = FAILSAFE_SHORT; @@ -18,7 +18,7 @@ void failsafe_short_on_event() case AUTO: case LOITER: - if(g.short_fs_action != 1) { + if(g.short_fs_action == 1) { set_mode(RTL); } break; @@ -32,10 +32,11 @@ void failsafe_short_on_event() SendDebugln(control_mode, DEC); } -void failsafe_long_on_event() +static void failsafe_long_on_event() { - // This is how to handle a short loss of control signal failsafe. + // This is how to handle a long loss of control signal failsafe. SendDebug_P("Failsafe - Long event on"); + APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC switch(control_mode) { case MANUAL: @@ -48,7 +49,7 @@ void failsafe_long_on_event() case AUTO: case LOITER: case CIRCLE: - if(g.long_fs_action != 1) { + if(g.long_fs_action == 1) { set_mode(RTL); } break; @@ -59,7 +60,7 @@ void failsafe_long_on_event() } } -void failsafe_short_off_event() +static void failsafe_short_off_event() { // We're back in radio contact SendDebug_P("Failsafe - Short event off"); @@ -74,15 +75,16 @@ void failsafe_short_off_event() reset_I(); } -void low_battery_event(void) +#if BATTERY_EVENT == ENABLED +static void low_battery_event(void) { gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); set_mode(RTL); g.throttle_cruise = THROTTLE_CRUISE; } +#endif - -void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY +static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY { if(event_repeat == 0 || (millis() - event_timer) < event_delay) return; @@ -108,17 +110,17 @@ void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPE } } -void relay_on() +static void relay_on() { PORTL |= B00000100; } -void relay_off() +static void relay_off() { PORTL &= ~B00000100; } -void relay_toggle() +static void relay_toggle() { PORTL ^= B00000100; } diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 6090510d23..487ffe4a3b 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -3,7 +3,7 @@ //**************************************************************** // Function that will calculate the desired direction to fly and distance //**************************************************************** -void navigate() +static void navigate() { // do not navigate with corrupt data // --------------------------------- @@ -63,7 +63,7 @@ void calc_distance_error() } #endif -void calc_airspeed_errors() +static void calc_airspeed_errors() { // XXX excess casting here if(control_mode>=AUTO && airspeed_nudge > 0) { @@ -75,7 +75,7 @@ void calc_airspeed_errors() } } -void calc_bearing_error() +static void calc_bearing_error() { if(takeoff_complete == true || g.compass_enabled == true) { bearing_error = nav_bearing - dcm.yaw_sensor; @@ -89,7 +89,7 @@ void calc_bearing_error() bearing_error = wrap_180(bearing_error); } -void calc_altitude_error() +static void calc_altitude_error() { if(control_mode == AUTO && offset_altitude != 0) { // limit climb rates @@ -127,38 +127,39 @@ void calc_altitude_error() altitude_error = target_altitude - current_loc.alt; } -long wrap_360(long error) +static long wrap_360(long error) { if (error > 36000) error -= 36000; if (error < 0) error += 36000; return error; } -long wrap_180(long error) +static long wrap_180(long error) { if (error > 18000) error -= 36000; if (error < -18000) error += 36000; return error; } -void update_loiter() +static void update_loiter() { float power; if(wp_distance <= g.loiter_radius){ power = float(wp_distance) / float(g.loiter_radius); + power = constrain(power, 0.5, 1); 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); + power = constrain(power, 0.5, 1); //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{ @@ -166,35 +167,27 @@ void update_loiter() } update_crosstrack(); +*/ nav_bearing = wrap_360(nav_bearing); } -void update_crosstrack(void) +static void update_crosstrack(void) { // Crosstrack Error // ---------------- if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line + crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing); } } -void reset_crosstrack() +static void reset_crosstrack() { crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } -long get_altitude_above_home(void) // TO DO - this function has been deprecated - check if any useage vs removal -{ - // This is the altitude above the home location - // The GPS gives us altitude at Sea Level - // if you slope soar, you should see a negative number sometimes - // ------------------------------------------------------------- - return current_loc.alt - home.alt; -} - -long get_distance(struct Location *loc1, struct Location *loc2) +static long get_distance(struct Location *loc1, struct Location *loc2) { if(loc1->lat == 0 || loc1->lng == 0) return -1; @@ -205,12 +198,7 @@ long get_distance(struct Location *loc1, struct Location *loc2) return sqrt(sq(dlat) + sq(dlong)) * .01113195; } -long get_alt_distance(struct Location *loc1, struct Location *loc2) -{ - return abs(loc1->alt - loc2->alt); -} - -long get_bearing(struct Location *loc1, struct Location *loc2) +static long get_bearing(struct Location *loc1, struct Location *loc2) { long off_x = loc2->lng - loc1->lng; long off_y = (loc2->lat - loc1->lat) * scaleLongUp; @@ -218,14 +206,3 @@ long get_bearing(struct Location *loc1, struct Location *loc2) if (bearing < 0) bearing += 36000; return bearing; } - -Vector3f get_location_vector(struct Location *uav, struct Location *target) -{ - Vector3 v; - v.x = (target->lng-uav->lng) * cos((uav->lat+target->lat)/2)*.01113195; - v.y = (target->lat-uav->lat)*.01113195; - v.z = (uav->alt-target->alt); - return v; -} - - diff --git a/ArduPlane/planner.pde b/ArduPlane/planner.pde index 9497110381..3154f18084 100644 --- a/ArduPlane/planner.pde +++ b/ArduPlane/planner.pde @@ -8,18 +8,19 @@ static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details -const struct Menu::command planner_menu_commands[] PROGMEM = { +static const struct Menu::command planner_menu_commands[] PROGMEM = { {"gcs", planner_gcs}, }; // A Macro to create the Menu MENU(planner_menu, "planner", planner_menu_commands); -int8_t +static int8_t planner_mode(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); planner_menu.run(); + return 0; } static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv) @@ -44,4 +45,6 @@ planner_gcs(uint8_t argc, const Menu::arg *argv) loopcount++; } } -} + return 0; +} + diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 0e0028ab90..dd346d8f91 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -2,21 +2,18 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling -void init_rc_in() +static void init_rc_in() { // set rc reversing update_servo_switches(); - g.channel_roll.set_reverse(reverse_roll); - g.channel_pitch.set_reverse(reverse_pitch); - g.channel_rudder.set_reverse(reverse_rudder); // set rc channel ranges - g.channel_roll.set_angle(ROLL_SERVO_MAX); - g.channel_pitch.set_angle(PITCH_SERVO_MAX); - g.channel_rudder.set_angle(RUDDER_SERVO_MAX); + g.channel_roll.set_angle(SERVO_MAX); + g.channel_pitch.set_angle(SERVO_MAX); + g.channel_rudder.set_angle(SERVO_MAX); g.channel_throttle.set_range(0, 100); // set rc dead zones @@ -26,13 +23,49 @@ void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - g.rc_5.set_range(0,1000); - g.rc_6.set_range(0,1000); - g.rc_7.set_range(0,1000); - g.rc_8.set_range(0,1000); + if (g_rc_function[RC_Channel_aux::k_mount_yaw]) { + g_rc_function[RC_Channel_aux::k_mount_yaw]->set_range( + g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); + } + if (g_rc_function[RC_Channel_aux::k_mount_pitch]) { + g_rc_function[RC_Channel_aux::k_mount_pitch]->set_range( + g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); + } + if (g_rc_function[RC_Channel_aux::k_mount_roll]) { + g_rc_function[RC_Channel_aux::k_mount_roll]->set_range( + g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); + } + if (g_rc_function[RC_Channel_aux::k_cam_trigger]) { + g_rc_function[RC_Channel_aux::k_cam_trigger]->set_range( + g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); + } + if (g_rc_function[RC_Channel_aux::k_cam_open]) { + g_rc_function[RC_Channel_aux::k_cam_open]->set_range( + g_rc_function[RC_Channel_aux::k_cam_open]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_cam_open]->angle_max / 10); + } + if (g_rc_function[RC_Channel_aux::k_flap]) { + g_rc_function[RC_Channel_aux::k_flap]->set_range(0,100); + } + if (g_rc_function[RC_Channel_aux::k_flap_auto]) { + g_rc_function[RC_Channel_aux::k_flap_auto]->set_range(0,100); + } + if (g_rc_function[RC_Channel_aux::k_aileron]) { + g_rc_function[RC_Channel_aux::k_aileron]->set_angle(SERVO_MAX); + } + if (g_rc_function[RC_Channel_aux::k_flaperon]) { + g_rc_function[RC_Channel_aux::k_flaperon]->set_range(0,100); + } + if (g_rc_function[RC_Channel_aux::k_egg_drop]) { + g_rc_function[RC_Channel_aux::k_egg_drop]->set_range(0,100); + } } -void init_rc_out() +static void init_rc_out() { APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); @@ -57,17 +90,17 @@ void init_rc_out() APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); } -void read_radio() +static void read_radio() { ch1_temp = APM_RC.InputCh(CH_ROLL); ch2_temp = APM_RC.InputCh(CH_PITCH); - if(mix_mode == 0){ + if(g.mix_mode == 0){ g.channel_roll.set_pwm(ch1_temp); g.channel_pitch.set_pwm(ch2_temp); }else{ - g.channel_roll.set_pwm(reverse_elevons * (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) - reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500); - g.channel_pitch.set_pwm((reverse_ch2_elevon * int(ch2_temp - elevon2_trim) + reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500); + g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); + g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); } g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); @@ -87,7 +120,7 @@ void read_radio() g.channel_throttle.servo_out = g.channel_throttle.control_in; if (g.channel_throttle.servo_out > 50) { - if(airspeed_enabled == true) { + if(g.airspeed_enabled == true) { airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); } else { throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); @@ -106,7 +139,7 @@ void read_radio() */ } -void control_failsafe(uint16_t pwm) +static void control_failsafe(uint16_t pwm) { if(g.throttle_fs_enabled == 0) return; @@ -121,7 +154,7 @@ void control_failsafe(uint16_t pwm) //Check for failsafe and debounce funky reads } else if (g.throttle_fs_enabled) { - if (pwm < g.throttle_fs_value){ + if (pwm < (unsigned)g.throttle_fs_value){ // we detect a failsafe from radio // throttle has dropped below the mark failsafeCounter++; @@ -153,15 +186,17 @@ void control_failsafe(uint16_t pwm) } } -void trim_control_surfaces() +static void trim_control_surfaces() { read_radio(); // Store control surface trim values // --------------------------------- - if(mix_mode == 0){ + if(g.mix_mode == 0){ g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; + if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + }else{ elevon1_trim = ch1_temp; elevon2_trim = ch2_temp; @@ -177,9 +212,10 @@ void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); + if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } -void trim_radio() +static void trim_radio() { for (int y = 0; y < 30; y++) { read_radio(); @@ -187,11 +223,12 @@ void trim_radio() // Store the trim values // --------------------- - if(mix_mode == 0){ + if(g.mix_mode == 0){ g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; + if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -207,5 +244,59 @@ void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); + if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } +// map a function to a servo channel +static void aux_servo_out(RC_Channel_aux* ch, unsigned char ch_nr) +{ + switch(ch->function) + { + case RC_Channel_aux::k_none: // disabled + return; + break; + case RC_Channel_aux::k_mount_yaw: // mount yaw (pan) + case RC_Channel_aux::k_mount_pitch: // mount pitch (tilt) + case RC_Channel_aux::k_mount_roll: // mount roll + case RC_Channel_aux::k_cam_trigger: // camera trigger + case RC_Channel_aux::k_cam_open: // camera open + case RC_Channel_aux::k_flap: // flaps + case RC_Channel_aux::k_flap_auto: // flaps automated + case RC_Channel_aux::k_aileron: // aileron + case RC_Channel_aux::k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) + case RC_Channel_aux::k_egg_drop: // egg drop + case RC_Channel_aux::k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning + break; + case RC_Channel_aux::k_manual: // manual + ch->radio_out = ch->radio_in; + break; + } + + APM_RC.OutputCh(ch_nr, ch->radio_out); +} + +// update the g_rc_function array from pointers to rc_x channels +static void update_aux_servo_function() +{ + RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos + aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)g.rc_5.function.get(); + aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)g.rc_6.function.get(); + aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)g.rc_7.function.get(); + aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)g.rc_8.function.get(); + + // Assume that no auxiliary function is used + for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) + { + g_rc_function[i] = NULL; + } + + // assign the RC channel to each function + g_rc_function[aux_servo_function[CH_5]] = &g.rc_5; + g_rc_function[aux_servo_function[CH_6]] = &g.rc_6; + g_rc_function[aux_servo_function[CH_7]] = &g.rc_7; + g_rc_function[aux_servo_function[CH_8]] = &g.rc_8; + +#if CAMERA == ENABLED + camera_mount.update_mount_type(); +#endif +} diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index d74ffcb060..3f7001d1ad 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -5,21 +5,17 @@ void ReadSCP1000(void) {} -void init_barometer(void) +static void init_barometer(void) { - int flashcount; + int flashcount = 0; long ground_pressure = 0; int ground_temperature; - #if HIL_MODE == HIL_MODE_SENSORS - hil.update(); // look for inbound hil packets for initialization - #endif - while(ground_pressure == 0){ barometer.Read(); // Get initial data from absolute pressure sensor ground_pressure = barometer.Press; ground_temperature = barometer.Temp; - delay(20); + mavlink_delay(20); //Serial.printf("barometer.Press %ld\n", barometer.Press); } @@ -33,7 +29,7 @@ void init_barometer(void) ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l; ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; - delay(20); + mavlink_delay(20); if(flashcount == 5) { digitalWrite(C_LED_PIN, LOW); digitalWrite(A_LED_PIN, HIGH); @@ -53,11 +49,11 @@ void init_barometer(void) g.ground_temperature.set_and_save(ground_temperature / 10.0f); abs_pressure = ground_pressure; -Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); - SendDebugln_P("barometer calibration complete."); + Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); + gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); } -long read_barometer(void) +static long read_barometer(void) { float x, scaling, temp; @@ -73,30 +69,38 @@ long read_barometer(void) } // in M/S * 100 -void read_airspeed(void) +static void read_airspeed(void) { #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed + if (g.airspeed_offset == 0) { + // runtime enabling of airspeed, we need to do instant + // calibration before we can use it. This isn't as + // accurate as the 50 point average in zero_airspeed(), + // but it is better than using it uncalibrated + airspeed_raw = (float)adc.Ch(AIRSPEED_CH); + g.airspeed_offset.set_and_save(airspeed_raw); + } airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75); - airspeed_pressure = max(((int)airspeed_raw - airspeed_offset), 0); + airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0); airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100; #endif calc_airspeed_errors(); } -void zero_airspeed(void) +static void zero_airspeed(void) { airspeed_raw = (float)adc.Ch(AIRSPEED_CH); - for(int c = 0; c < 50; c++){ + for(int c = 0; c < 10; c++){ delay(20); airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10); } - airspeed_offset = airspeed_raw; + g.airspeed_offset.set_and_save(airspeed_raw); } #endif // HIL_MODE != HIL_MODE_ATTITUDE -void read_battery(void) +static void read_battery(void) { battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; @@ -114,7 +118,7 @@ void read_battery(void) current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; } - #if BATTERY_EVENT == 1 + #if BATTERY_EVENT == ENABLED if(battery_voltage < LOW_VOLTAGE) low_battery_event(); if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); #endif diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 7d35d953c6..2841e8b787 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#if CLI_ENABLED == ENABLED + // Functions called from the setup menu static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); @@ -11,7 +13,7 @@ static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); // Command/function table for the setup menu -const struct Menu::command setup_menu_commands[] PROGMEM = { +static const struct Menu::command setup_menu_commands[] PROGMEM = { // command function called // ======= =============== {"reset", setup_factory}, @@ -28,7 +30,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { MENU(setup_menu, "setup", setup_menu_commands); // Called from the top-level menu to run the setup menu. -int8_t +static int8_t setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance @@ -41,6 +43,7 @@ setup_mode(uint8_t argc, const Menu::arg *argv) // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); + return 0; } // Print the current configuration. @@ -48,7 +51,6 @@ setup_mode(uint8_t argc, const Menu::arg *argv) static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { - uint8_t i; // clear the area print_blanks(8); @@ -73,8 +75,6 @@ setup_show(uint8_t argc, const Menu::arg *argv) static int8_t setup_factory(uint8_t argc, const Menu::arg *argv) { - - uint8_t i; int c; Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); @@ -185,7 +185,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { - byte switchPosition, oldSwitchPosition, mode; + byte switchPosition, mode = 0; Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); @@ -201,10 +201,10 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) if (oldSwitchPosition != switchPosition){ // force position 5 to MANUAL if (switchPosition > 4) { - g.flight_modes[switchPosition] = MANUAL; + flight_modes[switchPosition] = MANUAL; } // update our current mode - mode = g.flight_modes[switchPosition]; + mode = flight_modes[switchPosition]; // update the user print_switch(switchPosition, mode); @@ -228,13 +228,11 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode != FLY_BY_WIRE_B && mode != AUTO && mode != RTL && - mode != LOITER && - mode != TAKEOFF && - mode != LAND) + mode != LOITER) { if (mode < MANUAL) - mode = LAND; - else if (mode >LAND) + mode = LOITER; + else if (mode >LOITER) mode = MANUAL; else mode += radioInputSwitch; @@ -245,7 +243,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode = MANUAL; // save new mode - g.flight_modes[switchPosition] = mode; + flight_modes[switchPosition] = mode; // print new mode print_switch(switchPosition, mode); @@ -254,7 +252,8 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) // escape hatch if(Serial.available() > 0){ // save changes - g.flight_modes.save(); + for (mode=0; mode<6; mode++) + flight_modes[mode].save(); report_flight_modes(); print_done(); return (0); @@ -267,13 +266,13 @@ setup_declination(uint8_t argc, const Menu::arg *argv) { compass.set_declination(radians(argv[1].f)); report_compass(); + return 0; } static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) { - uint8_t i; int c; Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ")); @@ -292,9 +291,13 @@ static int8_t setup_compass(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("on"))) { - g.compass_enabled = true; - bool junkbool = compass.init(); - + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + if (!compass.init()) { + Serial.println_P(PSTR("Compass initialisation failed!")); + g.compass_enabled = false; + } else { + g.compass_enabled = true; + } } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.compass_enabled = false; @@ -323,50 +326,11 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv) return 0; } -/***************************************************************************/ -// CLI defaults -/***************************************************************************/ - -void -default_flight_modes() -{ - g.flight_modes[0] = FLIGHT_MODE_1; - g.flight_modes[1] = FLIGHT_MODE_2; - g.flight_modes[2] = FLIGHT_MODE_3; - g.flight_modes[3] = FLIGHT_MODE_4; - g.flight_modes[4] = FLIGHT_MODE_5; - g.flight_modes[5] = FLIGHT_MODE_6; - g.flight_modes.save(); -} - -void -default_log_bitmask() -{ - - // convenience macro for testing LOG_* and setting LOGBIT_* - #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) - - g.log_bitmask = - LOGBIT(ATTITUDE_FAST) | - LOGBIT(ATTITUDE_MED) | - LOGBIT(GPS) | - LOGBIT(PM) | - LOGBIT(CTUN) | - LOGBIT(NTUN) | - LOGBIT(MODE) | - LOGBIT(RAW) | - LOGBIT(CMD) | - LOGBIT(CUR); - #undef LOGBIT - - g.log_bitmask.save(); -} - /***************************************************************************/ // CLI reports /***************************************************************************/ -void report_batt_monitor() +static void report_batt_monitor() { //print_blanks(2); Serial.printf_P(PSTR("Batt Mointor\n")); @@ -378,7 +342,7 @@ void report_batt_monitor() if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); print_blanks(2); } -void report_radio() +static void report_radio() { //print_blanks(2); Serial.printf_P(PSTR("Radio\n")); @@ -388,7 +352,7 @@ void report_radio() print_blanks(2); } -void report_gains() +static void report_gains() { //print_blanks(2); Serial.printf_P(PSTR("Gains\n")); @@ -406,7 +370,7 @@ void report_gains() Serial.printf_P(PSTR("nav roll:\n")); print_PID(&g.pidNavRoll); - Serial.printf_P(PSTR("nav pitch airpseed:\n")); + Serial.printf_P(PSTR("nav pitch airspeed:\n")); print_PID(&g.pidNavPitchAirspeed); Serial.printf_P(PSTR("energry throttle:\n")); @@ -418,7 +382,7 @@ void report_gains() print_blanks(2); } -void report_xtrack() +static void report_xtrack() { //print_blanks(2); Serial.printf_P(PSTR("Crosstrack\n")); @@ -431,7 +395,7 @@ void report_xtrack() print_blanks(2); } -void report_throttle() +static void report_throttle() { //print_blanks(2); Serial.printf_P(PSTR("Throttle\n")); @@ -450,7 +414,7 @@ void report_throttle() print_blanks(2); } -void report_imu() +static void report_imu() { //print_blanks(2); Serial.printf_P(PSTR("IMU\n")); @@ -461,16 +425,32 @@ void report_imu() print_blanks(2); } -void report_compass() +static void report_compass() { //print_blanks(2); - Serial.printf_P(PSTR("Compass\n")); + Serial.printf_P(PSTR("Compass: ")); + + switch (compass.product_id) { + case AP_COMPASS_TYPE_HMC5883L: + Serial.println_P(PSTR("HMC5883L")); + break; + case AP_COMPASS_TYPE_HMC5843: + Serial.println_P(PSTR("HMC5843")); + break; + case AP_COMPASS_TYPE_HIL: + Serial.println_P(PSTR("HIL")); + break; + default: + Serial.println_P(PSTR("??")); + break; + } + print_divider(); print_enabled(g.compass_enabled); // mag declination - Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), + Serial.printf_P(PSTR("Mag Declination: %4.4f\n"), degrees(compass.get_declination())); Vector3f offsets = compass.get_offsets(); @@ -483,14 +463,14 @@ void report_compass() print_blanks(2); } -void report_flight_modes() +static void report_flight_modes() { //print_blanks(2); Serial.printf_P(PSTR("Flight modes\n")); print_divider(); for(int i = 0; i < 6; i++ ){ - print_switch(i, g.flight_modes[i]); + print_switch(i, flight_modes[i]); } print_blanks(2); } @@ -499,7 +479,7 @@ void report_flight_modes() // CLI utilities /***************************************************************************/ -void +static void print_PID(PID * pid) { Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), @@ -509,7 +489,7 @@ print_PID(PID * pid) (long)pid->imax()); } -void +static void print_radio_values() { Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); @@ -523,20 +503,20 @@ print_radio_values() } -void +static void print_switch(byte p, byte m) { Serial.printf_P(PSTR("Pos %d: "),p); Serial.println(flight_mode_strings[m]); } -void +static void print_done() { Serial.printf_P(PSTR("\nSaved Settings\n\n")); } -void +static void print_blanks(int num) { while(num > 0){ @@ -545,7 +525,7 @@ print_blanks(int num) } } -void +static void print_divider(void) { for (int i = 0; i < 40; i++) { @@ -554,7 +534,7 @@ print_divider(void) Serial.println(""); } -int8_t +static int8_t radio_input_switch(void) { static int8_t bouncer = 0; @@ -581,9 +561,9 @@ radio_input_switch(void) } -void zero_eeprom(void) +static void zero_eeprom(void) { - byte b; + byte b = 0; Serial.printf_P(PSTR("\nErasing EEPROM\n")); for (int i = 0; i < EEPROM_MAX_ADDR; i++) { eeprom_write_byte((uint8_t *) i, b); @@ -591,7 +571,7 @@ void zero_eeprom(void) Serial.printf_P(PSTR("done\n")); } -void print_enabled(bool b) +static void print_enabled(bool b) { if(b) Serial.printf_P(PSTR("en")); @@ -600,7 +580,7 @@ void print_enabled(bool b) Serial.printf_P(PSTR("abled\n")); } -void +static void print_accel_offsets(void) { Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), @@ -609,7 +589,7 @@ print_accel_offsets(void) (float)imu.az()); } -void +static void print_gyro_offsets(void) { Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), @@ -619,3 +599,4 @@ print_gyro_offsets(void) } +#endif // CLI_ENABLED diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index b447ef3b57..cbe7ddadd2 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -6,11 +6,13 @@ The init_ardupilot function processes everything we need for an in - air restart *****************************************************************************/ +#if CLI_ENABLED == ENABLED + // Functions called from the top-level menu -extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp -extern int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde +static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde +static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp +static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -28,7 +30,7 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) } // Command/function table for the top-level menu. -const struct Menu::command main_menu_commands[] PROGMEM = { +static const struct Menu::command main_menu_commands[] PROGMEM = { // command function called // ======= =============== {"logs", process_logs}, @@ -39,9 +41,11 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "ArduPilotMega", main_menu_commands); +MENU(main_menu, "APM trunk", main_menu_commands); -void init_ardupilot() +#endif // CLI_ENABLED + +static void init_ardupilot() { // Console serial port // @@ -73,16 +77,6 @@ void init_ardupilot() Serial1.begin(38400, 128, 16); #endif - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // - Serial3.begin(SERIAL3_BAUD, 128, 128); - Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %lu\n"), freeRAM()); @@ -92,7 +86,10 @@ void init_ardupilot() // if (!g.format_version.load()) { - Serial.println("\n\nEEPROM blank - all parameters are reset to default values.\n"); + Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n")); + delay(100); // wait for serial msg to flush + + AP_Var::erase_all(); // save the current format version g.format_version.set_and_save(Parameters::k_format_version); @@ -102,6 +99,7 @@ void init_ardupilot() Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" "\n\nForcing complete parameter reset..."), g.format_version.get(), Parameters::k_format_version); + delay(100); // wait for serial msg to flush // erase all parameters AP_Var::erase_all(); @@ -116,45 +114,48 @@ void init_ardupilot() AP_Var::load_all(); Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); - Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); + Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"), + AP_Var::get_memory_use(), (unsigned)g.num_resets); } - if (!g.flight_modes.load()) { - default_flight_modes(); - } - if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) { - default_log_bitmask(); - } + // keep a record of how many resets have happened. This can be + // used to detect in-flight resets + g.num_resets.set_and_save(g.num_resets+1); + + // Telemetry port. + // + // Not used if telemetry is going to the console. + // + // XXX for unidirectional protocols, we could (should) minimize + // the receive buffer, and the transmit buffer could also be + // shrunk for protocols that don't send large messages. + // + Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); mavlink_system.sysid = g.sysid_this_mav; -#if CAMERA == ENABLED - init_camera(); -#endif #if HIL_MODE != HIL_MODE_ATTITUDE adc.Init(); // APM ADC library initialization barometer.Init(); // APM Abs Pressure sensor initialization - // Autodetect airspeed sensor - if ((adc.Ch(AIRSPEED_CH) > 2000)&&(adc.Ch(AIRSPEED_CH) < 2900)) - { - airspeed_enabled = false; - } - else - { - airspeed_enabled = true; - } - if (g.compass_enabled==true) { - dcm.set_compass(&compass); - bool junkbool = compass.init(); - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference - //compass.set_declination(ToRad(get(PARAM_DECLINATION))); // TODO fix this to have a UI // set local difference between magnetic north and true north + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + if (!compass.init()) { + Serial.println_P(PSTR("Compass initialisation failed!")); + g.compass_enabled = false; + } else { + dcm.set_compass(&compass); + compass.get_offsets(); // load offsets to account for airframe magnetic interference + } } -#else - airspeed_enabled = true; + /* + Init is depricated - Jason + if(g.sonar_enabled){ + sonar.init(SONAR_PIN, &adc); + Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC); + } + */ #endif DataFlash.Init(); // DataFlash log initialization @@ -162,6 +163,7 @@ void init_ardupilot() // Do GPS init g_gps = &g_gps_driver; g_gps->init(); // GPS Initialization + g_gps->callback = mavlink_delay; // init the GCS #if GCS_PORT == 3 @@ -208,6 +210,7 @@ void init_ardupilot() // the system in an odd state, we don't let the user exit the top // menu; they must reset in order to fly. // +#if CLI_ENABLED == ENABLED if (digitalRead(SLIDE_SWITCH_PIN) == 0) { digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED Serial.printf_P(PSTR("\n" @@ -221,6 +224,7 @@ void init_ardupilot() main_menu.run(); } } +#endif // CLI_ENABLED if(g.log_bitmask != 0){ @@ -268,6 +272,8 @@ void init_ardupilot() Log_Write_Startup(TYPE_GROUNDSTART_MSG); } + set_mode(MANUAL); + // set the correct flight mode // --------------------------- reset_control_switch(); @@ -276,8 +282,10 @@ void init_ardupilot() //******************************************************************************** //This function does all the calibrations, etc. that we need during a ground start //******************************************************************************** -void startup_ground(void) +static void startup_ground(void) { + set_mode(INITIALISING); + gcs.send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); #if(GROUND_START_DELAY > 0) @@ -306,7 +314,7 @@ void startup_ground(void) trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. #if HIL_MODE != HIL_MODE_ATTITUDE -if (airspeed_enabled == true) +if (g.airspeed_enabled == true) { // initialize airspeed sensor // -------------------------- @@ -349,7 +357,7 @@ else gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } -void set_mode(byte mode) +static void set_mode(byte mode) { if(control_mode == mode){ // don't switch modes if we are already in the correct mode. @@ -363,18 +371,11 @@ void set_mode(byte mode) switch(control_mode) { + case INITIALISING: case MANUAL: - break; - case CIRCLE: - break; - case STABILIZE: - break; - case FLY_BY_WIRE_A: - break; - case FLY_BY_WIRE_B: break; @@ -390,10 +391,8 @@ void set_mode(byte mode) do_loiter_at_location(); break; - case TAKEOFF: - break; - - case LAND: + case GUIDED: + set_guided_WP(); break; default: @@ -408,7 +407,7 @@ void set_mode(byte mode) Log_Write_Mode(control_mode); } -void check_long_failsafe() +static void check_long_failsafe() { // only act on changes // ------------------- @@ -433,7 +432,7 @@ void check_long_failsafe() } } -void check_short_failsafe() +static void check_short_failsafe() { // only act on changes // ------------------- @@ -442,7 +441,7 @@ void check_short_failsafe() failsafe_short_on_event(); } } - + if(failsafe == FAILSAFE_SHORT){ if(!ch3_failsafe) { failsafe_short_off_event(); @@ -451,20 +450,19 @@ void check_short_failsafe() } -void startup_IMU_ground(void) +static void startup_IMU_ground(void) { #if HIL_MODE != HIL_MODE_ATTITUDE - uint16_t store = 0; - SendDebugln(" Warming up ADC..."); - delay(500); + gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); + mavlink_delay(500); // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! // ----------------------- demo_servos(2); - SendDebugln(" Beginning IMU calibration; do not move plane"); - delay(1000); + gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); + mavlink_delay(1000); - imu.init(IMU::COLD_START); + imu.init(IMU::COLD_START, mavlink_delay); dcm.set_centripetal(1); // read Baro pressure at ground @@ -479,7 +477,7 @@ void startup_IMU_ground(void) } -void update_GPS_light(void) +static void update_GPS_light(void) { // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- @@ -507,15 +505,16 @@ void update_GPS_light(void) } -void resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; +static void resetPerfData(void) { + mainLoop_count = 0; + G_Dt_max = 0; dcm.gyro_sat_count = 0; imu.adc_constraints = 0; dcm.renorm_sqrt_count = 0; dcm.renorm_blowup_count = 0; - gps_fix_count = 0; - perf_mon_timer = millis(); + gps_fix_count = 0; + pmTest1 = 0; + perf_mon_timer = millis(); } @@ -526,7 +525,7 @@ void resetPerfData(void) { * be larger than HP or you'll be in big trouble! The smaller the gap, the more * careful you need to be. Julian Gall 6 - Feb - 2009. */ -unsigned long freeRAM() { +static unsigned long freeRAM() { uint8_t * heapptr, * stackptr; stackptr = (uint8_t *)malloc(4); // use stackptr temporarily heapptr = stackptr; // save value of heap pointer @@ -535,3 +534,20 @@ unsigned long freeRAM() { return stackptr - heapptr; } + +/* + map from a 8 bit EEPROM baud rate to a real baud rate + */ +static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) +{ + switch (rate) { + case 9: return 9600; + case 19: return 19200; + case 38: return 38400; + case 57: return 57600; + case 111: return 111100; + case 115: return 115200; + } + Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); + return default_baud; +} diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 9200012f40..926e95ca1a 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -1,4 +1,6 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if CLI_ENABLED == ENABLED // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. @@ -26,7 +28,7 @@ static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details -const struct Menu::command test_menu_commands[] PROGMEM = { +static const struct Menu::command test_menu_commands[] PROGMEM = { {"pwm", test_radio_pwm}, {"radio", test_radio}, {"failsafe", test_failsafe}, @@ -64,14 +66,15 @@ const struct Menu::command test_menu_commands[] PROGMEM = { // A Macro to create the Menu MENU(test_menu, "test", test_menu_commands); -int8_t +static int8_t test_mode(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("Test Mode\n\n")); test_menu.run(); + return 0; } -void print_hit_enter() +static void print_hit_enter() { Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); } @@ -104,7 +107,7 @@ 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"), + Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.channel_roll.radio_in, g.channel_pitch.radio_in, g.channel_throttle.radio_in, @@ -147,7 +150,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) // write out the servo PWM values // ------------------------------ - set_servos_4(); + set_servos(); Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.channel_roll.control_in, @@ -255,7 +258,7 @@ if (g.battery_monitoring == 4) { // write out the servo PWM values // ------------------------------ - set_servos_4(); + set_servos(); if(Serial.available() > 0){ return (0); @@ -315,11 +318,11 @@ test_wp(uint8_t argc, const Menu::arg *argv) return (0); } -void -test_wp_print(struct Location *cmd, byte index) +static void +test_wp_print(struct Location *cmd, byte wp_index) { Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), - (int)index, + (int)wp_index, (int)cmd->id, (int)cmd->options, (int)cmd->p1, @@ -376,20 +379,24 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); + if (!g.switch_enable) { + Serial.println_P(PSTR("dip switches disabled, using EEPROM")); + } + while(1){ delay(100); update_servo_switches(); - if (mix_mode == 0) { + if (g.mix_mode == 0) { Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), - (int)reverse_roll, - (int)reverse_pitch, - (int)reverse_rudder); + (int)g.channel_roll.get_reverse(), + (int)g.channel_pitch.get_reverse(), + (int)g.channel_rudder.get_reverse()); } else { Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), - (int)reverse_elevons, - (int)reverse_ch1_elevon, - (int)reverse_ch2_elevon); + (int)g.reverse_elevons, + (int)g.reverse_ch1_elevon, + (int)g.reverse_ch2_elevon); } if(Serial.available() > 0){ return (0); @@ -525,13 +532,29 @@ test_gyro(uint8_t argc, const Menu::arg *argv) static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { + if (!g.compass_enabled) { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } + + compass.set_orientation(MAG_ORIENTATION); + if (!compass.init()) { + Serial.println_P(PSTR("Compass initialisation failed!")); + return 0; + } + dcm.set_compass(&compass); + report_compass(); + + // we need the DCM initialised for this test + imu.init(IMU::COLD_START); + int counter = 0; - if(g.compass_enabled) { //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); - print_hit_enter(); + print_hit_enter(); - while(1){ + while(1) { delay(20); if (millis() - fast_loopTimer > 19) { delta_ms_fast_loop = millis() - fast_loopTimer; @@ -542,40 +565,38 @@ test_mag(uint8_t argc, const Menu::arg *argv) // --- dcm.update_DCM(G_Dt); - if(g.compass_enabled) { - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - medium_loopCounter = 0; - } - } + medium_loopCounter++; + if(medium_loopCounter == 5){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + medium_loopCounter = 0; + } counter++; if (counter>20) { - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z, - maggy.x, - maggy.y, - maggy.z); - counter=0; -} + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z, + maggy.x, + maggy.y, + maggy.z); + counter=0; + } + } + if (Serial.available() > 0) { + break; + } + } - } - if(Serial.available() > 0){ - return (0); - } - } - } else { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); - } + // save offsets. This allows you to get sane offset values using + // the CLI before you go flying. + Serial.println_P(PSTR("saving offsets")); + compass.save_offsets(); + return (0); } #endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS @@ -588,14 +609,11 @@ test_mag(uint8_t argc, const Menu::arg *argv) static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv) { + unsigned airspeed_ch = adc.Ch(AIRSPEED_CH); // Serial.println(adc.Ch(AIRSPEED_CH)); - if ((adc.Ch(AIRSPEED_CH) > 2000) && (adc.Ch(AIRSPEED_CH) < 2900)){ - airspeed_enabled = false; - }else{ - airspeed_enabled = true; - } + Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch); - if (airspeed_enabled == false){ + if (g.airspeed_enabled == false){ Serial.printf_P(PSTR("airspeed: ")); print_enabled(false); return (0); @@ -614,11 +632,8 @@ test_airspeed(uint8_t argc, const Menu::arg *argv) if(Serial.available() > 0){ return (0); } - if(Serial.available() > 0){ - return (0); } } - } } @@ -669,3 +684,5 @@ test_rawgps(uint8_t argc, const Menu::arg *argv) } } #endif // HIL_MODE == HIL_MODE_DISABLED + +#endif // CLI_ENABLED diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index f19ba7fe93..5629e0ee3e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1,184 +1,197 @@ -#include "AP_Mount.h" +#include +#include -AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name): -_group(key, name), -_mountMode(&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name -_mountType(&_group, 0, 0, name ? PSTR("TYPE") : 0), -_dcm(dcm); -_gps(gps); +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function + +AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) { - + _dcm=dcm; + _gps=gps; } -void AP_Mount::SetPitchYaw(Location targetGPSLocation) +void AP_Mount::set_pitch_yaw(int pitchCh, int yawCh) { - _targetGPSLocation=targetGPSLocation; +} + +void AP_Mount::set_GPS_target(Location targetGPSLocation) +{ + _target_GPS_location=targetGPSLocation; //set mode - _mountMode=gps; + _mount_mode=k_gps_target; //update mount position - UpDateMount(); + update_mount(); } -void AP_Mount::SetGPSTarget(Location targetGPSLocation) +void AP_Mount::set_assisted(int roll, int pitch, int yaw) { - _targetGPSLocation=targetGPSLocation; - - //set mode - _mountMode=gps; - - //update mount position - UpDateMount(); -} - -void AP_Mount::SetAssisted(int roll, int pitch, int yaw) -{ - _AssistAngles.x = roll; - _AssistAngles.y = pitch; - _AssistAngles.z = yaw; + _assist_angles.x = roll; + _assist_angles.y = pitch; + _assist_angles.z = yaw; //set mode - _mountMode=assisted; + _mount_mode=k_assisted; //update mount position - UpDateMount(); + update_mount(); } //sets the servo angles for FPV, note angles are * 100 -void AP_Mount::SetMountFreeRoam(int roll, int pitch, int yaw) +void AP_Mount::set_mount_free_roam(int roll, int pitch, int yaw) { - _RoamAngles.x=roll; - _RoamAngles.y=pitch; - _RoamAngles.z=yaw; + _roam_angles.x=roll; + _roam_angles.y=pitch; + _roam_angles.z=yaw; //set mode - _mountMode=roam; + _mount_mode=k_roam; //now update mount position - UpDateMount(); + update_mount(); } //sets the servo angles for landing, note angles are * 100 -void AP_Mount::SetMountLanding(int roll, int pitch, int yaw) +void AP_Mount::set_mount_landing(int roll, int pitch, int yaw) { - _LandingAngles.x=roll; - _LandingAngles.y=pitch; - _LandingAngles.z=yaw; + _landing_angles.x=roll; + _landing_angles.y=pitch; + _landing_angles.z=yaw; //set mode - _mountMode=landing; + _mount_mode=k_landing; //now update mount position - UpDateMount(); + update_mount(); } -void AP_Mount::SetNone() +void AP_Mount::set_none() { //set mode - _mountMode=none; + _mount_mode=k_none; //now update mount position - UpDateMount(); + update_mount(); } -void AP_Mount::UpDateMount() +void AP_Mount::update_mount() { - switch(_mountMode) + Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs + Vector3f targ; //holds target vector, var is used as temp in calcs + + switch(_mount_mode) { - case gps: + case k_gps_target: { if(_gps->fix) { - CalcGPSTargetVector(&_targetGPSLocation); + calc_GPS_target_vector(&_target_GPS_location); } m = _dcm->get_dcm_transposed(); - targ = m*_GPSVector; - this->CalcMountAnglesFromVector(*targ) + targ = m*_GPS_vector; + roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll + pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch break; } - case stabilise: + case k_stabilise: { - //to do + // TODO replace this simplified implementation with a proper one + roll_angle = -_dcm->roll_sensor; + pitch_angle = -_dcm->pitch_sensor; + yaw_angle = -_dcm->yaw_sensor; break; } - case roam: + case k_roam: { - pitchAngle=100*_RoamAngles.y; - rollAngle=100*_RoamAngles.x; - yawAngle=100*_RoamAngles.z; + roll_angle=100*_roam_angles.x; + pitch_angle=100*_roam_angles.y; + yaw_angle=100*_roam_angles.z; break; } - case assisted: + case k_assisted: { m = _dcm->get_dcm_transposed(); - targ = m*_AssistVector; - this->CalcMountAnglesFromVector(*targ) + //rotate vector + targ = m*_assist_vector; + roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll + pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch break; } - case landing: + case k_landing: { - pitchAngle=100*_RoamAngles.y; - rollAngle=100*_RoamAngles.x; - yawAngle=100*_RoamAngles.z; + roll_angle=100*_roam_angles.x; + pitch_angle=100*_roam_angles.y; + yaw_angle=100*_roam_angles.z; break; } - case none: + case k_none: { //do nothing break; } + case k_manual: // radio manual control + if (g_rc_function[RC_Channel_aux::k_mount_roll]) + roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in, + g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min, + g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max, + g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min, + g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max); + if (g_rc_function[RC_Channel_aux::k_mount_pitch]) + pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in, + g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min, + g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max, + g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min, + g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max); + if (g_rc_function[RC_Channel_aux::k_mount_yaw]) + yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in, + g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min, + g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max, + g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min, + g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max); + break; default: { //do nothing break; } } + + // write the results to the servos + // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic + if (g_rc_function[RC_Channel_aux::k_mount_roll]) + g_rc_function[RC_Channel_aux::k_mount_roll]->closest_limit(roll_angle/10); + if (g_rc_function[RC_Channel_aux::k_mount_pitch]) + g_rc_function[RC_Channel_aux::k_mount_pitch]->closest_limit(pitch_angle/10); + if (g_rc_function[RC_Channel_aux::k_mount_yaw]) + g_rc_function[RC_Channel_aux::k_mount_yaw]->closest_limit(yaw_angle/10); } -void AP_Mount::SetMode(MountMode mode) +void AP_Mount::set_mode(MountMode mode) { - _mountMode=mode; + _mount_mode=mode; } -void AP_Mount::CalcGPSTargetVector(struct Location *target) +void AP_Mount::calc_GPS_target_vector(struct Location *target) { - _targetVector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195; - _targetVector.y = (target->lat-_gps->latitude)*.01113195; - _targetVector.z = (_gps->altitude-target->alt); + _GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195; + _GPS_vector.y = (target->lat-_gps->latitude)*.01113195; + _GPS_vector.z = (_gps->altitude-target->alt); } -void AP_Mount::CalcMountAnglesFromVector(Vector3f *targ) +void +AP_Mount::update_mount_type() { - switch(_mountType) + // Auto-detect the mount gimbal type depending on the functions assigned to the servos + if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) { - case pitch_yaw: - { - //need to tidy up maths for below - pitchAngle = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; - yawAngle = 9000 + atan2(-targ.y, targ.x) * 5729.57795; - break; - } - case pitch_roll: - { - pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch - rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll - break; - } - case pitch_roll_yaw: - { - //to do - break; - } - case none: - { - //do nothing - break; - } - default: - { - //do nothing - break; - } + _mount_type = k_pan_tilt; + } + if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL)) + { + _mount_type = k_tilt_roll; + } + if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) + { + _mount_type = k_pan_tilt_roll; } } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 4ee2930edc..6f1c202885 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -5,7 +5,7 @@ * * * Author: Joe Holdsworth; * * Ritchie Wilson; * -* Amiclair Lucus; * +* Amilcar Lucas; * * * * Purpose: Move a 2 or 3 axis mount attached to vehicle, * * Used for mount to track targets or stabilise * @@ -13,8 +13,6 @@ * * * Usage: Use in main code to control mounts attached to * * vehicle. * -* 1. initialise class * -* 2. setMounttype * * * * *Comments: All angles in degrees * 100, distances in meters* * unless otherwise stated. * @@ -22,78 +20,71 @@ #ifndef AP_Mount_H #define AP_Mount_H -#include +//#include #include #include #include class AP_Mount { -protected: - AP_Var_group _group; // must be before all vars to keep ctor init order correct - public: //Constructors - AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name); + AP_Mount(GPS *gps, AP_DCM *dcm); //enums enum MountMode{ - gps = 0, - stabilise = 1, //note the correct english spelling :) - roam = 2, - assisted = 3, - landing = 4, - none = 5 + k_gps_target = 0, + k_stabilise = 1, //note the correct English spelling :) + k_roam = 2, + k_assisted = 3, + k_landing = 4, + k_none = 5, + k_manual = 6 }; enum MountType{ - pitch_yaw = 0, - pitch_roll = 1, - pitch_roll_yaw = 2, - none = 3; + k_pan_tilt = 0, //yaw-pitch + k_tilt_roll = 1, //pitch-roll + k_pan_tilt_roll = 2, //yaw-pitch-roll }; //Accessors - //void SetPitchYaw(int pitchCh, int yawCh); - //void SetPitchRoll(int pitchCh, int rollCh); - //void SetPitchRollYaw(int pitchCh, int rollCh, int yawCh); + void set_pitch_yaw(int pitchCh, int yawCh); + void set_pitch_roll(int pitchCh, int rollCh); + void set_pitch_roll_yaw(int pitchCh, int rollCh, int yawCh); - void SetGPSTarget(Location targetGPSLocation); //used to tell the mount to track GPS location - void SetAssisted(int roll, int pitch, int yaw); - void SetMountFreeRoam(int roll, int pitch, int yaw);//used in the FPV for example, - void SetMountLanding(int roll, int pitch, int yaw); //set mount landing position - void SetNone(); + void set_GPS_target(Location targetGPSLocation); //used to tell the mount to track GPS location + void set_assisted(int roll, int pitch, int yaw); + void set_mount_free_roam(int roll, int pitch, int yaw);//used in the FPV for example, + void set_mount_landing(int roll, int pitch, int yaw); //set mount landing position + void set_none(); //methods - void UpDateMount(); - void SetMode(MountMode mode); + void update_mount(); + void update_mount_type(); //Auto-detect the mount gimbal type depending on the functions assigned to the servos + void set_mode(MountMode mode); - int pitchAngle; //degrees*100 - int rollAngle; //degrees*100 - int yawAngle; //degrees*100 - -private: + int pitch_angle; //degrees*100 + int roll_angle; //degrees*100 + int yaw_angle; //degrees*100 +protected: //methods - void CalcGPSTargetVector(struct Location *target); - void CalcMountAnglesFromVector(Vector3f *targ); + void calc_GPS_target_vector(struct Location *target); //void CalculateDCM(int roll, int pitch, int yaw); //members AP_DCM *_dcm; GPS *_gps; - MountMode _mountMode; - MountType _mountType; + MountMode _mount_mode; + MountType _mount_type; - struct Location _targetGPSLocation; - Vector3f _GPSVector; //target vector calculated stored in meters + struct Location _target_GPS_location; + Vector3f _GPS_vector; //target vector calculated stored in meters - Vector3i _RoamAngles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _LandingAngles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw + Vector3i _roam_angles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw + Vector3i _landing_angles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _AssistAngles; //used to keep angles that user has supplied from assisted targetting - Vector3f _AssistVector; //used to keep vector calculated from _AssistAngles - - Matrix3f _m; //holds 3 x 3 matrix, var is used as temp in calcs - Vector3f _targ; //holds target vector, var is used as temp in calcs + Vector3i _assist_angles; //used to keep angles that user has supplied from assisted targeting + Vector3f _assist_vector; //used to keep vector calculated from _AssistAngles }; #endif \ No newline at end of file diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 24fae0f5ea..d5cf7105fa 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -218,3 +218,38 @@ RC_Channel::norm_output() else return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim); } + +int16_t +RC_Channel_aux::closest_limit(int16_t angle) +{ + // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic + int16_t min = angle_min / 10; + int16_t max = angle_max / 10; + + // Make sure the angle lies in the interval [-180 .. 180[ degrees + while (angle < -1800) angle += 3600; + while (angle >= 1800) angle -= 3600; + + // Make sure the angle limits lie in the interval [-180 .. 180[ degrees + while (min < -1800) min += 3600; + while (min >= 1800) min -= 3600; + while (max < -1800) max += 3600; + while (max >= 1800) max -= 3600; + set_range(min, max); + + // If the angle is outside servo limits, saturate the angle to the closest limit + // On a circle the closest angular position must be carefully calculated to account for wrap-around + if ((angle < min) && (angle > max)){ + // angle error if min limit is used + int16_t err_min = min - angle + (anglemax?0:3600); // add 360 degrees if on the "wrong side" + angle = err_min +// TODO is this include really necessary ? #include /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel{ - private: + protected: AP_Var_group _group; // must be before all vars to keep ctor init order correct public: @@ -103,6 +104,48 @@ class RC_Channel{ int16_t _low; }; +/// @class RC_Channel_aux +/// @brief Object managing one aux. RC channel (CH5-8), with information about its function +class RC_Channel_aux : public RC_Channel{ +public: + /// Constructor + /// + /// @param key EEPROM storage key for the channel trim parameters. + /// @param name Optional name for the group. + /// + RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) : + RC_Channel(key, name), + function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name + angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection + angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection + {} + + typedef enum + { + k_none = 0, // 0=disabled + k_mount_yaw = 1, // 1=mount yaw (pan) + k_mount_pitch = 2, // 2=mount pitch (tilt) + k_mount_roll = 3, // 3=mount roll + k_cam_trigger = 4, // 4=camera trigger + k_cam_open = 5, // 5=camera open + k_flap = 6, // 6=flap + k_flap_auto = 7, // 7=flap automated + k_aileron = 8, // 8=aileron + k_flaperon = 9, // 9=flaperon (flaps and aileron combined, needs two independent servos one for each wing) + k_egg_drop = 10, // 10=egg drop + k_manual = 11, // 11=manual, just pass-thru the RC in signal + k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) + } Aux_servo_function_t; + + AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func + + AP_Aux_srv_func function; // 0=disabled, 1=mount yaw (pan), 2=mount pitch (tilt), 3=mount roll, 4=camera trigger, 5=camera open, 6=flap, 7=flap auto, 8=aileron, 9=flaperon, 10=egg drop, 11=manual + AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units + AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units + + int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval +}; + #endif From 0b9dbf464563f51790ffc9cd438384fde459e8db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:06:46 +1000 Subject: [PATCH 06/74] mavlink: import new version with AP_ADC packet this adds the AP_ADC packet which gives us raw ADC values --- .../include/ardupilotmega/ardupilotmega.h | 7 +- .../ardupilotmega/mavlink_msg_ap_adc.h | 254 ++++++++++++++++++ .../include/ardupilotmega/testsuite.h | 54 ++++ .../include/ardupilotmega/version.h | 2 +- .../GCS_MAVLink/include/common/version.h | 2 +- .../message_definitions/ardupilotmega.xml | 10 + .../GCS_MAVLink/message_definitions/test.xml | 31 +++ 7 files changed, 355 insertions(+), 5 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h create mode 100644 libraries/GCS_MAVLink/message_definitions/test.xml diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index 1272728607..e650159666 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -48,6 +48,7 @@ extern "C" { #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" #include "./mavlink_msg_meminfo.h" +#include "./mavlink_msg_ap_adc.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h new file mode 100644 index 0000000000..d42edb15d3 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h @@ -0,0 +1,254 @@ +// MESSAGE AP_ADC PACKING + +#define MAVLINK_MSG_ID_AP_ADC 153 + +typedef struct __mavlink_ap_adc_t +{ + uint16_t adc1; ///< ADC output 1 + uint16_t adc2; ///< ADC output 2 + uint16_t adc3; ///< ADC output 3 + uint16_t adc4; ///< ADC output 4 + uint16_t adc5; ///< ADC output 5 + uint16_t adc6; ///< ADC output 6 +} mavlink_ap_adc_t; + +#define MAVLINK_MSG_ID_AP_ADC_LEN 12 +#define MAVLINK_MSG_ID_153_LEN 12 + + + +#define MAVLINK_MESSAGE_INFO_AP_ADC { \ + "AP_ADC", \ + 6, \ + { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ + { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ + { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ + { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ + { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ + { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ + } \ +} + + +/** + * @brief Pack a ap_adc message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; + return mavlink_finalize_message(msg, system_id, component_id, 12); +} + +/** + * @brief Pack a ap_adc message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12); +} + +/** + * @brief Encode a ap_adc struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + +/** + * @brief Send a ap_adc message + * @param chan MAVLink channel to send the message + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12); +#endif +} + +#endif + +// MESSAGE AP_ADC UNPACKING + + +/** + * @brief Get field adc1 from ap_adc message + * + * @return ADC output 1 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field adc2 from ap_adc message + * + * @return ADC output 2 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field adc3 from ap_adc message + * + * @return ADC output 3 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field adc4 from ap_adc message + * + * @return ADC output 4 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field adc5 from ap_adc message + * + * @return ADC output 5 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field adc6 from ap_adc message + * + * @return ADC output 6 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Decode a ap_adc message into a struct + * + * @param msg The message to decode + * @param ap_adc C-struct to decode the message contents into + */ +static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) +{ +#if MAVLINK_NEED_BYTE_SWAP + ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); + ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); + ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); + ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); + ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); + ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); +#else + memcpy(ap_adc, _MAV_PAYLOAD(msg), 12); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index cd59317f8f..6a2e90cae5 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -185,11 +185,65 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ap_adc_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 17651, + 17755, + }; + mavlink_ap_adc_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.adc1 = packet_in.adc1; + packet1.adc2 = packet_in.adc2; + packet1.adc3 = packet_in.adc3; + packet1.adc4 = packet_in.adc4; + packet1.adc5 = packet_in.adc5; + packet1.adc6 = packet_in.adc6; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ifree memory + + raw ADC output + ADC output 1 + ADC output 2 + ADC output 3 + ADC output 4 + ADC output 5 + ADC output 6 + + diff --git a/libraries/GCS_MAVLink/message_definitions/test.xml b/libraries/GCS_MAVLink/message_definitions/test.xml new file mode 100644 index 0000000000..43a11e3d13 --- /dev/null +++ b/libraries/GCS_MAVLink/message_definitions/test.xml @@ -0,0 +1,31 @@ + + + 3 + + + Test all field types + char + string + uint8_t + uint16_t + uint32_t + uint64_t + int8_t + int16_t + int32_t + int64_t + float + double + uint8_t_array + uint16_t_array + uint32_t_array + uint64_t_array + int8_t_array + int16_t_array + int32_t_array + int64_t_array + float_array + double_array + + + From 52dc0a915a38c29da378076d2fdd7fb689ef32f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:08:18 +1000 Subject: [PATCH 07/74] added support for AP_ADC MAVLink packet this adds AP_ADC, which sends raw ADC 16 bit values for all 6 ADC channels at the Extra3 MAVLink stream rate. Extra3 was previously unused --- ArduPlane/GCS_Mavlink.pde | 3 +-- ArduPlane/Mavlink_Common.h | 16 +++++++++++++++- ArduPlane/defines.h | 1 + 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index db34c15389..c1546d95a4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -131,8 +131,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - // Available datastream - //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); + send_message(MSG_RAW_ADC); } } } diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..4c2147b2c4 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -344,6 +344,20 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; } + + case MSG_RAW_ADC: + { + CHECK_PAYLOAD_SIZE(AP_ADC); + mavlink_msg_ap_adc_send(chan, + analogRead(BATTERY_PIN1), + analogRead(BATTERY_PIN2), + analogRead(BATTERY_PIN3), + analogRead(BATTERY_PIN4), + analogRead(AN4), + analogRead(AN5)); + break; + } + default: break; } @@ -351,7 +365,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } -#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of +#define MAX_DEFERRED_MESSAGES 18 // should be at least equal to number of // switch types in mavlink_try_send_message() static struct mavlink_queue { uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b48deed2da..3929123647 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -176,6 +176,7 @@ #define MSG_GPS_RAW 0x64 #define MSG_SERVO_OUT 0x70 +#define MSG_RAW_ADC 0x71 #define MSG_PIN_REQUEST 0x80 #define MSG_PIN_SET 0x81 From b3d4bb14780762bd566dac82878d31bb0f33b2d9 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 12:51:48 +0200 Subject: [PATCH 08/74] Again remove some more files from the APM_Camera branch --- libraries/AP_Camera/Camera.cpp | 277 -------------------------------- libraries/AP_Camera/Camera.h | 71 -------- libraries/AP_Mount/keywords.txt | 9 -- 3 files changed, 357 deletions(-) delete mode 100644 libraries/AP_Camera/Camera.cpp delete mode 100644 libraries/AP_Camera/Camera.h delete mode 100644 libraries/AP_Mount/keywords.txt diff --git a/libraries/AP_Camera/Camera.cpp b/libraries/AP_Camera/Camera.cpp deleted file mode 100644 index 2c9bb928fd..0000000000 --- a/libraries/AP_Camera/Camera.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include "Camera.h" -#include "../RC_Channel/RC_Channel.h" - -void -Camera::move() -{ - Vector3 target_vector(0,0,1); // x, y, z to target before rotating to planes axis, values are in meters - - //decide what happens to camera depending on camera mode - switch(mode) - { - case 0: - //do nothing, i.e lock camera in place - return; - break; - case 1: - //stabilize - target_vector.x=0; //east to west gives +tive value (i.e. longitude) - target_vector.y=0; //south to north gives +tive value (i.e. latitude) - target_vector.z=100; //downwards is +tive - break; - case 2: - //track target - if(g_gps->fix) - { - target_vector=get_location_vector(¤t_loc,&camera_target); - } - break; - case 3: // radio manual control - case 4: // test (level the camera and point north) - break; // see code 25 lines bellow - } - - Matrix3f m = dcm.get_dcm_transposed(); - Vector3 targ = m*target_vector; //to do: find out notion of x y convention - switch(gimbal_type) - { - case 0: // pitch & roll (tilt & roll) - cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch - cam_roll = degrees(atan2(targ.y, targ.z)); //roll - break; - - case 1: // yaw & pitch (pan & tilt) - cam_pitch = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; - cam_yaw = 9000 + atan2(-targ.y, targ.x) * 5729.57795; - break; - -/* case 2: // pitch, roll & yaw - not started - cam_ritch = 0; - cam_yoll = 0; - cam_paw = 0; - break; */ - - } - - //some camera modes overwrite the gimbal_type calculations - switch(mode) - { - case 3: // radio manual control - if (rc_function[CAM_PITCH]) - cam_pitch = map(rc_function[CAM_PITCH]->radio_in, - rc_function[CAM_PITCH]->radio_min, - rc_function[CAM_PITCH]->radio_max, - rc_function[CAM_PITCH]->angle_min, - rc_function[CAM_PITCH]->radio_max); - if (rc_function[CAM_ROLL]) - cam_roll = map(rc_function[CAM_ROLL]->radio_in, - rc_function[CAM_ROLL]->radio_min, - rc_function[CAM_ROLL]->radio_max, - rc_function[CAM_ROLL]->angle_min, - rc_function[CAM_ROLL]->radio_max); - if (rc_function[CAM_YAW]) - cam_yaw = map(rc_function[CAM_YAW]->radio_in, - rc_function[CAM_YAW]->radio_min, - rc_function[CAM_YAW]->radio_max, - rc_function[CAM_YAW]->angle_min, - rc_function[CAM_YAW]->radio_max); - break; - case 4: // test (level the camera and point north) - cam_pitch = -dcm.pitch_sensor; - cam_yaw = dcm.yaw_sensor; // do not invert because the servo is mounted upside-down on my system - // TODO: the "trunk" code can invert using parameters, but this branch still can't - cam_roll = -dcm.roll_sensor; - break; - } - - #if CAM_DEBUG == ENABLED - //for debugging purposes - Serial.println(); - Serial.print("current_loc: lat: "); - Serial.print(current_loc.lat); - Serial.print(", lng: "); - Serial.print(current_loc.lng); - Serial.print(", alt: "); - Serial.print(current_loc.alt); - Serial.println(); - Serial.print("target_loc: lat: "); - Serial.print(camera_target.lat); - Serial.print(", lng: "); - Serial.print(camera_target.lng); - Serial.print(", alt: "); - Serial.print(camera_target.alt); - Serial.print(", distance: "); - Serial.print(get_distance(¤t_loc,&camera_target)); - Serial.print(", bearing: "); - Serial.print(get_bearing(¤t_loc,&camera_target)); - Serial.println(); - Serial.print("dcm_angles: roll: "); - Serial.print(degrees(dcm.roll)); - Serial.print(", pitch: "); - Serial.print(degrees(dcm.pitch)); - Serial.print(", yaw: "); - Serial.print(degrees(dcm.yaw)); - Serial.println(); - Serial.print("target_vector: x: "); - Serial.print(target_vector.x,2); - Serial.print(", y: "); - Serial.print(target_vector.y,2); - Serial.print(", z: "); - Serial.print(target_vector.z,2); - Serial.println(); - Serial.print("rotated_target_vector: x: "); - Serial.print(targ.x,2); - Serial.print(", y: "); - Serial.print(targ.y,2); - Serial.print(", z: "); - Serial.print(targ.z,2); - Serial.println(); - Serial.print("gimbal type 0: roll: "); - Serial.print(roll); - Serial.print(", pitch: "); - Serial.print(pitch); - Serial.println(); - /* Serial.print("gimbal type 1: pitch: "); - Serial.print(pan); - Serial.print(", roll: "); - Serial.print(tilt); - Serial.println(); */ - /* Serial.print("gimbal type 2: pitch: "); - Serial.print(ritch); - Serial.print(", roll: "); - Serial.print(yoll); - Serial.print(", yaw: "); - Serial.print(paw); - Serial.println(); */ -#endif -} - - -void -Camera::set_target(struct Location target) -{ - camera_target = target; -} - - -void -Camera::update_camera_gimbal_type() -{ - - // Auto detect the camera gimbal type depending on the functions assigned to the servos - if ((rc_function[CAM_YAW] == NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL)) - { - gimbal_type = 0; - } - if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] == NULL)) - { - gimbal_type = 1; - } - if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL)) - { - gimbal_type = 2; - } -} - -void -Camera::servo_pic() // Servo operated camera -{ - if (rc_function[CAM_TRIGGER]) - { - cam_trigger = rc_function[CAM_TRIGGER]->radio_max; - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles - } -} - -void -Camera::relay_pic() // basic relay activation -{ - relay_on(); - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles -} - -void -Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic == 10){ - servo_pic(); // triggering method - thr_pic = 0; - g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void -Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method - g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void -Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // To Do: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles -} - -// single entry point to take pictures -void -Camera::trigger_pic() -{ - switch (trigger_type) - { - case 0: - servo_pic(); // Servo operated camera - break; - case 1: - relay_pic(); // basic relay activation - break; - case 2: - throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - break; - case 3: - distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - break; - case 4: - NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - break; - } -} - -// de-activate the trigger after some delay, but without using a delay() function -void -Camera::trigger_pic_cleanup() -{ - if (keep_cam_trigg_active_cycles) - { - keep_cam_trigg_active_cycles --; - } - else - { - switch (trigger_type) - { - case 0: - case 2: - case 3: - if (rc_function[CAM_TRIGGER]) - { - cam_trigger = rc_function[CAM_TRIGGER]->radio_min; - } - break; - case 1: - relay_off(); - break; - case 4: - digitalWrite(camtrig, LOW); - break; - } - } -} diff --git a/libraries/AP_Camera/Camera.h b/libraries/AP_Camera/Camera.h deleted file mode 100644 index 88dc792748..0000000000 --- a/libraries/AP_Camera/Camera.h +++ /dev/null @@ -1,71 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file Camera.h -/// @brief Photo or video camera manager, with EEPROM-backed storage of constants. - -#ifndef CAMERA_H -#define CAMERA_H - -#include - -/// @class Camera -/// @brief Object managing a Photo or video camera -class Camera{ -protected: - AP_Var_group _group; // must be before all vars to keep ctor init order correct - -public: - /// Constructor - /// - /// @param key EEPROM storage key for the camera configuration parameters. - /// @param name Optional name for the group. - /// - Camera(AP_Var::Key key, const prog_char_t *name) : - _group(key, name), - mode (&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name - trigger_type(&_group, 1, 0, name ? PSTR("TRIGGER_MODE") : 0), - picture_time (0), // waypoint trigger variable - thr_pic (0), // timer variable for throttle_pic - camtrig (83), // PK6 chosen as it not near anything so safer for soldering -// camera_target (home), // use test target for now - gimbal_type (1), - keep_cam_trigg_active_cycles (0) - {} - - // move the camera depending on the camera mode - void move(); - - // single entry point to take pictures - void trigger_pic(); - - // de-activate the trigger after some delay, but without using a delay() function - void trigger_pic_cleanup(); - - // call this from time to time to make sure the correct gimbal type gets choosen - void update_camera_gimbal_type(); - - // set camera orientation target - void set_target(struct Location target); - - int picture_time; // waypoint trigger variable - -private: - uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active - struct Location camera_target; // point of interest for the camera to track -// struct Location GPS_mark; // GPS POI for position based triggering - int thr_pic; // timer variable for throttle_pic - int camtrig; // PK6 chosen as it not near anything so safer for soldering - - AP_Int8 mode; // 0=do nothing, 1=stabilize, 2=track target, 3=manual, 4=simple stabilize test - AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor - uint8_t gimbal_type; // 0=pitch & roll (tilt & roll), 1=yaw & pitch(pan & tilt), 2=pitch, roll & yaw (to be added) - - void servo_pic(); // Servo operated camera - void relay_pic(); // basic relay activation - void throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - void distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - void NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - -}; - -#endif /* CAMERA_H */ diff --git a/libraries/AP_Mount/keywords.txt b/libraries/AP_Mount/keywords.txt deleted file mode 100644 index 68fe325f17..0000000000 --- a/libraries/AP_Mount/keywords.txt +++ /dev/null @@ -1,9 +0,0 @@ -AP_Mount KEYWORD1 -SetGPSTarget KEYWORD2 -SetAssisted KEYWORD2 -SetAntenna KEYWORD2 -SetMountFPV KEYWORD2 -SetMountLanding KEYWORD2 -UpDateMount KEYWORD2 -SetMode KEYWORD2 - From f4998c36734aa12470962ef48bf324a6330710d9 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 13:26:29 +0200 Subject: [PATCH 09/74] Moved a function from radio.pde to the RC_Channel_aux library. Now its more readable and reusable --- ArduPlane/Attitude.pde | 8 ++++---- ArduPlane/radio.pde | 28 --------------------------- libraries/RC_Channel/RC_Channel.cpp | 30 +++++++++++++++++++++++++++++ libraries/RC_Channel/RC_Channel.h | 3 +++ 4 files changed, 37 insertions(+), 32 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index e5a109744d..07300a65b6 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -339,10 +339,10 @@ static void set_servos(void) APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos // Route configurable aux. functions to their respective servos - aux_servo_out(&g.rc_5, CH_5); - aux_servo_out(&g.rc_6, CH_6); - aux_servo_out(&g.rc_7, CH_7); - aux_servo_out(&g.rc_8, CH_8); + g.rc_5.output_ch(CH_5); + g.rc_6.output_ch(CH_6); + g.rc_7.output_ch(CH_7); + g.rc_8.output_ch(CH_8); #endif } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index dd346d8f91..f38d57cf1f 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -247,34 +247,6 @@ static void trim_radio() if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } -// map a function to a servo channel -static void aux_servo_out(RC_Channel_aux* ch, unsigned char ch_nr) -{ - switch(ch->function) - { - case RC_Channel_aux::k_none: // disabled - return; - break; - case RC_Channel_aux::k_mount_yaw: // mount yaw (pan) - case RC_Channel_aux::k_mount_pitch: // mount pitch (tilt) - case RC_Channel_aux::k_mount_roll: // mount roll - case RC_Channel_aux::k_cam_trigger: // camera trigger - case RC_Channel_aux::k_cam_open: // camera open - case RC_Channel_aux::k_flap: // flaps - case RC_Channel_aux::k_flap_auto: // flaps automated - case RC_Channel_aux::k_aileron: // aileron - case RC_Channel_aux::k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) - case RC_Channel_aux::k_egg_drop: // egg drop - case RC_Channel_aux::k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning - break; - case RC_Channel_aux::k_manual: // manual - ch->radio_out = ch->radio_in; - break; - } - - APM_RC.OutputCh(ch_nr, ch->radio_out); -} - // update the g_rc_function array from pointers to rc_x channels static void update_aux_servo_function() { diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index d5cf7105fa..2673f26e53 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -11,6 +11,7 @@ #include #include +#include #include "WProgram.h" #include "RC_Channel.h" @@ -253,3 +254,32 @@ RC_Channel_aux::closest_limit(int16_t angle) return angle; } + +// map a function to a servo channel and output it +void +RC_Channel_aux::output_ch(unsigned char ch_nr) +{ + switch(function) + { + case k_none: // disabled + return; + break; + case k_mount_yaw: // mount yaw (pan) + case k_mount_pitch: // mount pitch (tilt) + case k_mount_roll: // mount roll + case k_cam_trigger: // camera trigger + case k_cam_open: // camera open + case k_flap: // flaps + case k_flap_auto: // flaps automated + case k_aileron: // aileron + case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) + case k_egg_drop: // egg drop + case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning + break; + case k_manual: // manual + radio_out = radio_in; + break; + } + + APM_RC.OutputCh(ch_nr, radio_out); +} diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 58557d9dda..1519611c4b 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -144,6 +144,9 @@ public: AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval + + void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it + }; #endif From 59cbb8ec6f5391dca1ffabdae35c2992bbc325f1 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 13:47:09 +0200 Subject: [PATCH 10/74] Moved a camera mount function call, out of the servos update function --- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/radio.pde | 4 ---- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 2b744873a2..45b95b69ee 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -730,6 +730,9 @@ static void slow_loop() update_aux_servo_function(); +#if CAMERA == ENABLED + camera_mount.update_mount_type(); +#endif break; case 2: diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index f38d57cf1f..11513fba41 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -267,8 +267,4 @@ static void update_aux_servo_function() g_rc_function[aux_servo_function[CH_6]] = &g.rc_6; g_rc_function[aux_servo_function[CH_7]] = &g.rc_7; g_rc_function[aux_servo_function[CH_8]] = &g.rc_8; - -#if CAMERA == ENABLED - camera_mount.update_mount_type(); -#endif } From 95af8dc1722048e05ad55a60a60078cf964a3744 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:06:46 +1000 Subject: [PATCH 11/74] mavlink: import new version with AP_ADC packet this adds the AP_ADC packet which gives us raw ADC values --- .../include/ardupilotmega/ardupilotmega.h | 7 +- .../ardupilotmega/mavlink_msg_ap_adc.h | 254 ++++++++++++++++++ .../include/ardupilotmega/testsuite.h | 54 ++++ .../include/ardupilotmega/version.h | 2 +- .../GCS_MAVLink/include/common/version.h | 2 +- .../message_definitions/ardupilotmega.xml | 10 + .../GCS_MAVLink/message_definitions/test.xml | 31 +++ 7 files changed, 355 insertions(+), 5 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h create mode 100644 libraries/GCS_MAVLink/message_definitions/test.xml diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index 1272728607..e650159666 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -48,6 +48,7 @@ extern "C" { #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" #include "./mavlink_msg_meminfo.h" +#include "./mavlink_msg_ap_adc.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h new file mode 100644 index 0000000000..d42edb15d3 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h @@ -0,0 +1,254 @@ +// MESSAGE AP_ADC PACKING + +#define MAVLINK_MSG_ID_AP_ADC 153 + +typedef struct __mavlink_ap_adc_t +{ + uint16_t adc1; ///< ADC output 1 + uint16_t adc2; ///< ADC output 2 + uint16_t adc3; ///< ADC output 3 + uint16_t adc4; ///< ADC output 4 + uint16_t adc5; ///< ADC output 5 + uint16_t adc6; ///< ADC output 6 +} mavlink_ap_adc_t; + +#define MAVLINK_MSG_ID_AP_ADC_LEN 12 +#define MAVLINK_MSG_ID_153_LEN 12 + + + +#define MAVLINK_MESSAGE_INFO_AP_ADC { \ + "AP_ADC", \ + 6, \ + { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ + { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ + { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ + { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ + { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ + { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ + } \ +} + + +/** + * @brief Pack a ap_adc message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; + return mavlink_finalize_message(msg, system_id, component_id, 12); +} + +/** + * @brief Pack a ap_adc message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12); +} + +/** + * @brief Encode a ap_adc struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + +/** + * @brief Send a ap_adc message + * @param chan MAVLink channel to send the message + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12); +#endif +} + +#endif + +// MESSAGE AP_ADC UNPACKING + + +/** + * @brief Get field adc1 from ap_adc message + * + * @return ADC output 1 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field adc2 from ap_adc message + * + * @return ADC output 2 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field adc3 from ap_adc message + * + * @return ADC output 3 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field adc4 from ap_adc message + * + * @return ADC output 4 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field adc5 from ap_adc message + * + * @return ADC output 5 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field adc6 from ap_adc message + * + * @return ADC output 6 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Decode a ap_adc message into a struct + * + * @param msg The message to decode + * @param ap_adc C-struct to decode the message contents into + */ +static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) +{ +#if MAVLINK_NEED_BYTE_SWAP + ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); + ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); + ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); + ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); + ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); + ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); +#else + memcpy(ap_adc, _MAV_PAYLOAD(msg), 12); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index cd59317f8f..6a2e90cae5 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -185,11 +185,65 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ap_adc_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 17651, + 17755, + }; + mavlink_ap_adc_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.adc1 = packet_in.adc1; + packet1.adc2 = packet_in.adc2; + packet1.adc3 = packet_in.adc3; + packet1.adc4 = packet_in.adc4; + packet1.adc5 = packet_in.adc5; + packet1.adc6 = packet_in.adc6; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ifree memory + + raw ADC output + ADC output 1 + ADC output 2 + ADC output 3 + ADC output 4 + ADC output 5 + ADC output 6 + + diff --git a/libraries/GCS_MAVLink/message_definitions/test.xml b/libraries/GCS_MAVLink/message_definitions/test.xml new file mode 100644 index 0000000000..43a11e3d13 --- /dev/null +++ b/libraries/GCS_MAVLink/message_definitions/test.xml @@ -0,0 +1,31 @@ + + + 3 + + + Test all field types + char + string + uint8_t + uint16_t + uint32_t + uint64_t + int8_t + int16_t + int32_t + int64_t + float + double + uint8_t_array + uint16_t_array + uint32_t_array + uint64_t_array + int8_t_array + int16_t_array + int32_t_array + int64_t_array + float_array + double_array + + + From c65d038ccfbb52bf538f87e91ab8bde9df8500ff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:08:18 +1000 Subject: [PATCH 12/74] added support for AP_ADC MAVLink packet this adds AP_ADC, which sends raw ADC 16 bit values for all 6 ADC channels at the Extra3 MAVLink stream rate. Extra3 was previously unused --- ArduPlane/GCS_Mavlink.pde | 3 +-- ArduPlane/Mavlink_Common.h | 16 +++++++++++++++- ArduPlane/defines.h | 1 + 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index db34c15389..c1546d95a4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -131,8 +131,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - // Available datastream - //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); + send_message(MSG_RAW_ADC); } } } diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..4c2147b2c4 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -344,6 +344,20 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; } + + case MSG_RAW_ADC: + { + CHECK_PAYLOAD_SIZE(AP_ADC); + mavlink_msg_ap_adc_send(chan, + analogRead(BATTERY_PIN1), + analogRead(BATTERY_PIN2), + analogRead(BATTERY_PIN3), + analogRead(BATTERY_PIN4), + analogRead(AN4), + analogRead(AN5)); + break; + } + default: break; } @@ -351,7 +365,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } -#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of +#define MAX_DEFERRED_MESSAGES 18 // should be at least equal to number of // switch types in mavlink_try_send_message() static struct mavlink_queue { uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index fff75fda4f..a996ec0f48 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -162,6 +162,7 @@ #define MSG_GPS_RAW 0x64 #define MSG_SERVO_OUT 0x70 +#define MSG_RAW_ADC 0x71 #define MSG_PIN_REQUEST 0x80 #define MSG_PIN_SET 0x81 From a2c1caacf5b1df71f4fafb8fef3443e62a728347 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 23:24:50 +1000 Subject: [PATCH 13/74] Revert "added support for AP_ADC MAVLink packet" This reverts commit a65a2dda2c1bfa95437880227a3abddc95b329f5. I am reverting this not for any sane reason, but because my 3 APM boards now lock up on any I2C operation, and I don't know why. I can't see how this change could have caused it, but I don't want to take the chance. --- ArduPlane/GCS_Mavlink.pde | 3 +- ArduPlane/Mavlink_Common.h | 16 +- ArduPlane/defines.h | 1 - .../include/ardupilotmega/ardupilotmega.h | 7 +- .../ardupilotmega/mavlink_msg_ap_adc.h | 254 ------------------ .../include/ardupilotmega/testsuite.h | 54 ---- .../include/ardupilotmega/version.h | 2 +- .../GCS_MAVLink/include/common/version.h | 2 +- .../message_definitions/ardupilotmega.xml | 10 - .../GCS_MAVLink/message_definitions/test.xml | 31 --- 10 files changed, 8 insertions(+), 372 deletions(-) delete mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h delete mode 100644 libraries/GCS_MAVLink/message_definitions/test.xml diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index c1546d95a4..db34c15389 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -131,7 +131,8 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - send_message(MSG_RAW_ADC); + // Available datastream + //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 4c2147b2c4..3de4438cc2 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -344,20 +344,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; } - - case MSG_RAW_ADC: - { - CHECK_PAYLOAD_SIZE(AP_ADC); - mavlink_msg_ap_adc_send(chan, - analogRead(BATTERY_PIN1), - analogRead(BATTERY_PIN2), - analogRead(BATTERY_PIN3), - analogRead(BATTERY_PIN4), - analogRead(AN4), - analogRead(AN5)); - break; - } - default: break; } @@ -365,7 +351,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } -#define MAX_DEFERRED_MESSAGES 18 // should be at least equal to number of +#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of // switch types in mavlink_try_send_message() static struct mavlink_queue { uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 3929123647..b48deed2da 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -176,7 +176,6 @@ #define MSG_GPS_RAW 0x64 #define MSG_SERVO_OUT 0x70 -#define MSG_RAW_ADC 0x71 #define MSG_PIN_REQUEST 0x80 #define MSG_PIN_SET 0x81 diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index e650159666..1272728607 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -48,7 +48,6 @@ extern "C" { #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" #include "./mavlink_msg_meminfo.h" -#include "./mavlink_msg_ap_adc.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h deleted file mode 100644 index d42edb15d3..0000000000 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE AP_ADC PACKING - -#define MAVLINK_MSG_ID_AP_ADC 153 - -typedef struct __mavlink_ap_adc_t -{ - uint16_t adc1; ///< ADC output 1 - uint16_t adc2; ///< ADC output 2 - uint16_t adc3; ///< ADC output 3 - uint16_t adc4; ///< ADC output 4 - uint16_t adc5; ///< ADC output 5 - uint16_t adc6; ///< ADC output 6 -} mavlink_ap_adc_t; - -#define MAVLINK_MSG_ID_AP_ADC_LEN 12 -#define MAVLINK_MSG_ID_153_LEN 12 - - - -#define MAVLINK_MESSAGE_INFO_AP_ADC { \ - "AP_ADC", \ - 6, \ - { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ - { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ - { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ - { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ - { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ - { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ - } \ -} - - -/** - * @brief Pack a ap_adc message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; - return mavlink_finalize_message(msg, system_id, component_id, 12); -} - -/** - * @brief Pack a ap_adc message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12); -} - -/** - * @brief Encode a ap_adc struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ap_adc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) -{ - return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); -} - -/** - * @brief Send a ap_adc message - * @param chan MAVLink channel to send the message - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12); -#endif -} - -#endif - -// MESSAGE AP_ADC UNPACKING - - -/** - * @brief Get field adc1 from ap_adc message - * - * @return ADC output 1 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field adc2 from ap_adc message - * - * @return ADC output 2 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field adc3 from ap_adc message - * - * @return ADC output 3 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field adc4 from ap_adc message - * - * @return ADC output 4 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field adc5 from ap_adc message - * - * @return ADC output 5 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field adc6 from ap_adc message - * - * @return ADC output 6 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Decode a ap_adc message into a struct - * - * @param msg The message to decode - * @param ap_adc C-struct to decode the message contents into - */ -static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) -{ -#if MAVLINK_NEED_BYTE_SWAP - ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); - ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); - ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); - ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); - ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); - ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); -#else - memcpy(ap_adc, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index 6a2e90cae5..cd59317f8f 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -185,65 +185,11 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } -static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_ap_adc_t packet_in = { - 17235, - 17339, - 17443, - 17547, - 17651, - 17755, - }; - mavlink_ap_adc_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.adc1 = packet_in.adc1; - packet1.adc2 = packet_in.adc2; - packet1.adc3 = packet_in.adc3; - packet1.adc4 = packet_in.adc4; - packet1.adc5 = packet_in.adc5; - packet1.adc6 = packet_in.adc6; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; ifree memory - - raw ADC output - ADC output 1 - ADC output 2 - ADC output 3 - ADC output 4 - ADC output 5 - ADC output 6 - - diff --git a/libraries/GCS_MAVLink/message_definitions/test.xml b/libraries/GCS_MAVLink/message_definitions/test.xml deleted file mode 100644 index 43a11e3d13..0000000000 --- a/libraries/GCS_MAVLink/message_definitions/test.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - 3 - - - Test all field types - char - string - uint8_t - uint16_t - uint32_t - uint64_t - int8_t - int16_t - int32_t - int64_t - float - double - uint8_t_array - uint16_t_array - uint32_t_array - uint64_t_array - int8_t_array - int16_t_array - int32_t_array - int64_t_array - float_array - double_array - - - From 3c3764479f14efc3d93327217b1ef74939d1f8af Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 10 Sep 2011 12:53:03 -0600 Subject: [PATCH 14/74] Test commit Just a nonsense commit to check commit functionality to new repo --- ArduPlane/APM_Config.h.reference | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 6d8685b3e3..6236d93d21 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -1,4 +1,5 @@ // +// // Example and reference ArduPilot Mega configuration file // ======================================================= // From f3385d9a9a58f4da7330264f2dc80d5f7a6b05f8 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 10 Sep 2011 13:10:27 -0600 Subject: [PATCH 15/74] Remove prev change --- ArduPlane/APM_Config.h.reference | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 6236d93d21..6d8685b3e3 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -1,5 +1,4 @@ // -// // Example and reference ArduPilot Mega configuration file // ======================================================= // From 0a793a13274abd4b41fb7037b96535a5af6b48be Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 23:24:50 +1000 Subject: [PATCH 16/74] Revert "added support for AP_ADC MAVLink packet" This reverts commit a65a2dda2c1bfa95437880227a3abddc95b329f5. I am reverting this not for any sane reason, but because my 3 APM boards now lock up on any I2C operation, and I don't know why. I can't see how this change could have caused it, but I don't want to take the chance. --- ArduPlane/GCS_Mavlink.pde | 3 +- ArduPlane/Mavlink_Common.h | 16 +- ArduPlane/defines.h | 1 - .../include/ardupilotmega/ardupilotmega.h | 7 +- .../ardupilotmega/mavlink_msg_ap_adc.h | 254 ------------------ .../include/ardupilotmega/testsuite.h | 54 ---- .../include/ardupilotmega/version.h | 2 +- .../GCS_MAVLink/include/common/version.h | 2 +- .../message_definitions/ardupilotmega.xml | 10 - .../GCS_MAVLink/message_definitions/test.xml | 31 --- 10 files changed, 8 insertions(+), 372 deletions(-) delete mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h delete mode 100644 libraries/GCS_MAVLink/message_definitions/test.xml diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index c1546d95a4..db34c15389 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -131,7 +131,8 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - send_message(MSG_RAW_ADC); + // Available datastream + //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 4c2147b2c4..3de4438cc2 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -344,20 +344,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; } - - case MSG_RAW_ADC: - { - CHECK_PAYLOAD_SIZE(AP_ADC); - mavlink_msg_ap_adc_send(chan, - analogRead(BATTERY_PIN1), - analogRead(BATTERY_PIN2), - analogRead(BATTERY_PIN3), - analogRead(BATTERY_PIN4), - analogRead(AN4), - analogRead(AN5)); - break; - } - default: break; } @@ -365,7 +351,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } -#define MAX_DEFERRED_MESSAGES 18 // should be at least equal to number of +#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of // switch types in mavlink_try_send_message() static struct mavlink_queue { uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a996ec0f48..fff75fda4f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -162,7 +162,6 @@ #define MSG_GPS_RAW 0x64 #define MSG_SERVO_OUT 0x70 -#define MSG_RAW_ADC 0x71 #define MSG_PIN_REQUEST 0x80 #define MSG_PIN_SET 0x81 diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index e650159666..1272728607 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -48,7 +48,6 @@ extern "C" { #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" #include "./mavlink_msg_meminfo.h" -#include "./mavlink_msg_ap_adc.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h deleted file mode 100644 index d42edb15d3..0000000000 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE AP_ADC PACKING - -#define MAVLINK_MSG_ID_AP_ADC 153 - -typedef struct __mavlink_ap_adc_t -{ - uint16_t adc1; ///< ADC output 1 - uint16_t adc2; ///< ADC output 2 - uint16_t adc3; ///< ADC output 3 - uint16_t adc4; ///< ADC output 4 - uint16_t adc5; ///< ADC output 5 - uint16_t adc6; ///< ADC output 6 -} mavlink_ap_adc_t; - -#define MAVLINK_MSG_ID_AP_ADC_LEN 12 -#define MAVLINK_MSG_ID_153_LEN 12 - - - -#define MAVLINK_MESSAGE_INFO_AP_ADC { \ - "AP_ADC", \ - 6, \ - { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ - { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ - { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ - { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ - { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ - { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ - } \ -} - - -/** - * @brief Pack a ap_adc message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; - return mavlink_finalize_message(msg, system_id, component_id, 12); -} - -/** - * @brief Pack a ap_adc message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12); -} - -/** - * @brief Encode a ap_adc struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ap_adc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) -{ - return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); -} - -/** - * @brief Send a ap_adc message - * @param chan MAVLink channel to send the message - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12); -#endif -} - -#endif - -// MESSAGE AP_ADC UNPACKING - - -/** - * @brief Get field adc1 from ap_adc message - * - * @return ADC output 1 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field adc2 from ap_adc message - * - * @return ADC output 2 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field adc3 from ap_adc message - * - * @return ADC output 3 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field adc4 from ap_adc message - * - * @return ADC output 4 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field adc5 from ap_adc message - * - * @return ADC output 5 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field adc6 from ap_adc message - * - * @return ADC output 6 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Decode a ap_adc message into a struct - * - * @param msg The message to decode - * @param ap_adc C-struct to decode the message contents into - */ -static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) -{ -#if MAVLINK_NEED_BYTE_SWAP - ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); - ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); - ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); - ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); - ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); - ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); -#else - memcpy(ap_adc, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index 6a2e90cae5..cd59317f8f 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -185,65 +185,11 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } -static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_ap_adc_t packet_in = { - 17235, - 17339, - 17443, - 17547, - 17651, - 17755, - }; - mavlink_ap_adc_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.adc1 = packet_in.adc1; - packet1.adc2 = packet_in.adc2; - packet1.adc3 = packet_in.adc3; - packet1.adc4 = packet_in.adc4; - packet1.adc5 = packet_in.adc5; - packet1.adc6 = packet_in.adc6; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; ifree memory - - raw ADC output - ADC output 1 - ADC output 2 - ADC output 3 - ADC output 4 - ADC output 5 - ADC output 6 - - diff --git a/libraries/GCS_MAVLink/message_definitions/test.xml b/libraries/GCS_MAVLink/message_definitions/test.xml deleted file mode 100644 index 43a11e3d13..0000000000 --- a/libraries/GCS_MAVLink/message_definitions/test.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - 3 - - - Test all field types - char - string - uint8_t - uint16_t - uint32_t - uint64_t - int8_t - int16_t - int32_t - int64_t - float - double - uint8_t_array - uint16_t_array - uint32_t_array - uint64_t_array - int8_t_array - int16_t_array - int32_t_array - int64_t_array - float_array - double_array - - - From 7e0bff9cbe026f58a71c8ee98fddc2cb2be14581 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:35:23 +0200 Subject: [PATCH 17/74] Egg_drop has a 0..100 range --- ArduPlane/WP_activity.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde index d497409342..474644cb90 100644 --- a/ArduPlane/WP_activity.pde +++ b/ArduPlane/WP_activity.pde @@ -32,10 +32,10 @@ void egg_waypoint() if(g.waypoint_index == 3){ if(wp_distance < egg_dist){ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (-45*10.31); + g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 100; } }else{ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (45*10.31); + g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 0; } } } From d3280bdd16a2b6ac04ce4b2b8b6ee9278acd9ab1 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:36:47 +0200 Subject: [PATCH 18/74] Added comments explaining when should the function be used --- ArduPlane/radio.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 11513fba41..f989d147e7 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -248,6 +248,8 @@ static void trim_radio() } // update the g_rc_function array from pointers to rc_x channels +// This should be done periodically because the user might change the configuration and +// expects the changes to take effect instantly static void update_aux_servo_function() { RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos From 386e80eaf600a4225adb58d6650b228bb60c335b Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:57:27 +0200 Subject: [PATCH 19/74] Added comments and TODO --- libraries/RC_Channel/RC_Channel.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 1519611c4b..2cc4df73d3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -106,6 +106,7 @@ class RC_Channel{ /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function +/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements class RC_Channel_aux : public RC_Channel{ public: /// Constructor @@ -137,6 +138,8 @@ public: k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; + // TODO It would be great if the "packed" attribute could be added to this somehow + // It would probably save some memory. But it can only be added to enums and not to typedefs :( AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func AP_Aux_srv_func function; // 0=disabled, 1=mount yaw (pan), 2=mount pitch (tilt), 3=mount roll, 4=camera trigger, 5=camera open, 6=flap, 7=flap auto, 8=aileron, 9=flaperon, 10=egg drop, 11=manual From b36d0352f376b3e7a1ceb349782eaa9e5008b22b Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:58:25 +0200 Subject: [PATCH 20/74] Added a comment about lower half of the array --- ArduPlane/radio.pde | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index f989d147e7..f32dd5e367 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -252,6 +252,7 @@ static void trim_radio() // expects the changes to take effect instantly static void update_aux_servo_function() { + // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)g.rc_5.function.get(); aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)g.rc_6.function.get(); From 02bfd433903bee4fdb5667129a9e9f6e46d00120 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 10 Sep 2011 15:16:51 -0700 Subject: [PATCH 21/74] Fixes Alt hold mistake Moves tuning into runtime definable param Added more IMax to throttle Signed-off-by: Jason Short --- ArduCopter/ArduCopter.pde | 108 ++++++++++++++++++------------------ ArduCopter/Attitude.pde | 2 +- ArduCopter/Mavlink_Common.h | 12 ++-- ArduCopter/Parameters.h | 4 ++ ArduCopter/config.h | 4 +- ArduCopter/defines.h | 19 ++----- ArduCopter/radio.pde | 24 -------- ArduCopter/setup.pde | 23 ++++++++ ArduCopter/test.pde | 57 ++----------------- 9 files changed, 99 insertions(+), 154 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 31d427146a..423c1cfd7f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -296,6 +296,7 @@ static boolean motor_auto_armed; // if true, //int max_stabilize_dampener; // //int max_yaw_dampener; // static Vector3f omega; +float tuning_value; // LED output // ---------- @@ -507,7 +508,7 @@ static byte simple_timer; // for limiting the execution of flight mode thi static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav -static float load; // % MCU cycles used +//static float load; // % MCU cycles used static byte counter_one_herz; static bool GPS_enabled = false; @@ -530,7 +531,7 @@ void loop() //PORTK |= B00010000; delta_ms_fast_loop = millis() - fast_loopTimer; fast_loopTimer = millis(); - load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; + //load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator mainLoop_count++; @@ -907,16 +908,15 @@ static void slow_loop() // between 1 and 5 Hz #else gcs.send_message(MSG_LOCATION); - gcs.send_message(MSG_CPU_LOAD, load*100); + //gcs.send_message(MSG_CPU_LOAD, load*100); #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.data_stream_send(1,5); #endif - #if CHANNEL_6_TUNING != CH6_NONE + if(g.radio_tuning > 0) tuning(); - #endif // filter out the baro offset. //if(baro_alt_offset > 0) baro_alt_offset--; @@ -1301,7 +1301,7 @@ static void read_AHRS(void) hil.update(); #endif - dcm.update_DCM(G_Dt); + dcm.update_DCM(G_Dt);//, _tog); omega = dcm.get_gyro(); } @@ -1396,63 +1396,65 @@ adjust_altitude() } static void tuning(){ + tuning_value = (float)g.rc_6.control_in / 1000.0; - //Outer Loop : Attitude - #if CHANNEL_6_TUNING == CH6_STABILIZE_KP - g.pi_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0); - g.pi_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); + switch(g.radio_tuning){ + case CH6_STABILIZE_KP: + g.rc_6.set_range(0,8000); // 0 to 8 + g.pi_stabilize_roll.kP(tuning_value); + g.pi_stabilize_pitch.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI - g.pi_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0); - g.pi_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0); + case CH6_STABILIZE_KI: + g.rc_6.set_range(0,300); // 0 to .3 + tuning_value = (float)g.rc_6.control_in / 1000.0; + g.pi_stabilize_roll.kI(tuning_value); + g.pi_stabilize_pitch.kI(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_YAW_KP - g.pi_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0 + case CH6_RATE_KP: + g.rc_6.set_range(0,300); // 0 to .3 + g.pi_rate_roll.kP(tuning_value); + g.pi_rate_pitch.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_YAW_KI - g.pi_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0); + case CH6_RATE_KI: + g.rc_6.set_range(0,300); // 0 to .3 + g.pi_rate_roll.kI(tuning_value); + g.pi_rate_pitch.kI(tuning_value); + break; + case CH6_YAW_KP: + g.rc_6.set_range(0,1000); + g.pi_stabilize_yaw.kP(tuning_value); + break; - //Inner Loop : Rate - #elif CHANNEL_6_TUNING == CH6_RATE_KP - g.pi_rate_roll.kP((float)g.rc_6.control_in / 1000.0); - g.pi_rate_pitch.kP((float)g.rc_6.control_in / 1000.0); + case CH6_YAW_RATE_KP: + g.rc_6.set_range(0,1000); + g.pi_rate_yaw.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_RATE_KI - g.pi_rate_roll.kI((float)g.rc_6.control_in / 1000.0); - g.pi_rate_pitch.kI((float)g.rc_6.control_in / 1000.0); + case CH6_THROTTLE_KP: + g.rc_6.set_range(0,1000); + g.pi_throttle.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP - g.pi_rate_yaw.kP((float)g.rc_6.control_in / 1000.0); + case CH6_TOP_BOTTOM_RATIO: + g.rc_6.set_range(800,1000); // .8 to 1 + g.top_bottom_ratio = tuning_value; + break; - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI - g.pi_rate_yaw.kI((float)g.rc_6.control_in / 1000.0); + case CH6_RELAY: + g.rc_6.set_range(0,1000); + if (g.rc_6.control_in <= 600) relay_on(); + if (g.rc_6.control_in >= 400) relay_off(); + break; - - //Altitude Hold - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP - g.pi_throttle.kP((float)g.rc_6.control_in / 1000.0); // 0 to 1 - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD - g.pi_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 - - //Extras - #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO - g.top_bottom_ratio = (float)g.rc_6.control_in / 1000.0; - - #elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED - g.waypoint_speed_max = (float)g.rc_6.control_in / 1000.0; - - - #elif CHANNEL_6_TUNING == CH6_PMAX - g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000 - - // Simple relay control - #elif CHANNEL_6_TUNING == CH6_RELAY - if(g.rc_6.control_in <= 600) relay_on(); - if(g.rc_6.control_in >= 400) relay_off(); - - #endif + case CH6_TRAVERSE_SPEED: + g.rc_6.set_range(0,1000); + g.waypoint_speed_max = g.rc_6.control_in; + break; + } } static void update_nav_wp() diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index effcf76bc2..8a9512664d 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -102,7 +102,7 @@ get_nav_throttle(long z_error, int target_speed) rate_error = constrain(rate_error, -110, 110); throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); - return g.throttle_cruise + rate_error; + return g.throttle_cruise + throttle; } diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index f401d048ef..7d54f4ef80 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -87,7 +87,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ mode, nav_mode, status, - load * 1000, + 0, battery_voltage * 1000, battery_remaining, packet_drops); @@ -294,14 +294,14 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); Vector3f mag_offsets = compass.get_offsets(); - mavlink_msg_sensor_offsets_send(chan, + mavlink_msg_sensor_offsets_send(chan, mag_offsets.x, mag_offsets.y, mag_offsets.z, compass.get_declination(), - barometer.RawPress, + barometer.RawPress, barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), + imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az()); break; } @@ -353,8 +353,8 @@ static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t pa // see if we can send the deferred messages, if any while (q->num_deferred_messages != 0) { - if (!mavlink_try_send_message(chan, - q->deferred_messages[q->next_deferred_message], + if (!mavlink_try_send_message(chan, + q->deferred_messages[q->next_deferred_message], packet_drops)) { break; } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c8b8037991..9013557147 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -105,6 +105,7 @@ public: k_param_throttle_fs_value, k_param_throttle_cruise, k_param_esc_calibrate, + k_param_radio_tuning, #if FRAME_CONFIG == HELI_FRAME @@ -231,6 +232,8 @@ public: AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int8 compass_enabled; AP_Int8 esc_calibrate; + AP_Int8 radio_tuning; + AP_Int8 frame_orientation; AP_Float top_bottom_ratio; AP_Int8 optflow_enabled; @@ -324,6 +327,7 @@ public: log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), + radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 664ab801c9..1b3423e4ca 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -513,7 +513,7 @@ // Throttle control gains // #ifndef THROTTLE_P -# define THROTTLE_P 0.35 // +# define THROTTLE_P 0.6 // #endif #ifndef THROTTLE_I # define THROTTLE_I 0.10 // with 4m error, 12.5s windup @@ -522,7 +522,7 @@ //# define THROTTLE_D 0.6 // upped with filter //#endif #ifndef THROTTLE_IMAX -# define THROTTLE_IMAX 150 +# define THROTTLE_IMAX 300 #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6b485b146a..62339e3b79 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -138,13 +138,6 @@ #define CIRCLE 8 // AUTO control #define NUM_MODES 9 -// YAW debug -// --------- -#define YAW_HOLD 0 -#define YAW_BRAKE 1 -#define YAW_RATE 2 - - // CH_6 Tuning // ----------- #define CH6_NONE 0 @@ -152,20 +145,16 @@ #define CH6_STABILIZE_KP 1 #define CH6_STABILIZE_KI 2 #define CH6_YAW_KP 3 -#define CH6_YAW_KD 4 // Rate #define CH6_RATE_KP 5 #define CH6_RATE_KI 6 #define CH6_YAW_RATE_KP 7 -#define CH6_YAW_RATE_KD 8 // Altitude -#define CH6_THROTTLE_KP 9 -#define CH6_THROTTLE_KD 10 +#define CH6_THROTTLE_KP 8 // Extras -#define CH6_TOP_BOTTOM_RATIO 11 -#define CH6_PMAX 12 -#define CH6_RELAY 13 -#define CH6_TRAVERSE_SPEED 14 +#define CH6_TOP_BOTTOM_RATIO 9 +#define CH6_RELAY 10 +#define CH6_TRAVERSE_SPEED 11 // nav byte mask // ------------- diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 21e6758962..4025730e8c 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -35,30 +35,6 @@ static void init_rc_in() g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); - #if CHANNEL_6_TUNING == CH6_RATE_KP - g.rc_6.set_range(0,300); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_RATE_KI - g.rc_6.set_range(0,300); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KP - g.rc_6.set_range(0,8000); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI - g.rc_6.set_range(0,300); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP - g.rc_6.set_range(0,800); // 0 to .8 - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD - g.rc_6.set_range(0,500); // 0 to .5 - - #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO - g.rc_6.set_range(800,1000); // .8 to 1 - -/* #elif CHANNEL_6_TUNING == CH6_RELAY - g.rc_6.set_range(0,1000); // 0 to 1 */ - #endif } static void init_rc_out() diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 9b618dcadb..529297aa2d 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -13,6 +13,7 @@ static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); +static int8_t setup_tune (uint8_t argc, const Menu::arg *argv); //static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); @@ -41,6 +42,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { {"battery", setup_batt_monitor}, {"sonar", setup_sonar}, {"compass", setup_compass}, + {"tune", setup_tune}, // {"offsets", setup_mag_offset}, {"declination", setup_declination}, #ifdef OPTFLOW_ENABLED @@ -354,6 +356,15 @@ setup_declination(uint8_t argc, const Menu::arg *argv) return 0; } +static int8_t +setup_tune(uint8_t argc, const Menu::arg *argv) +{ + g.radio_tuning.set_and_save(argv[1].i); + report_tuning(); + return 0; +} + + static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) @@ -1131,3 +1142,15 @@ static void report_version() print_blanks(2); } + +static void report_tuning() +{ + Serial.printf_P(PSTR("\nTUNE:\n")); + print_divider(); + if (g.radio_tuning == 0){ + print_enabled(g.radio_tuning.get()); + }else{ + Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get()); + } + print_blanks(2); +} diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 3baaf3cc08..6dcd5614f4 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -636,59 +636,8 @@ test_tuning(uint8_t argc, const Menu::arg *argv) while(1){ delay(200); read_radio(); - - //Outer Loop : Attitude - #if CHANNEL_6_TUNING == CH6_NONE - Serial.printf_P(PSTR("disabled\n")); - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KP - Serial.printf_P(PSTR("stab kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI - Serial.printf_P(PSTR("stab kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_YAW_KP - Serial.printf_P(PSTR("yaw Hold kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); // range from 0 ~ 5.0 - - #elif CHANNEL_6_TUNING == CH6_YAW_KI - Serial.printf_P(PSTR("yaw Hold kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - //Inner Loop : Rate - #elif CHANNEL_6_TUNING == CH6_RATE_KP - Serial.printf_P(PSTR("rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_RATE_KI - Serial.printf_P(PSTR("rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP - Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI - Serial.printf_P(PSTR("yaw rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - - //Altitude Hold - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP - Serial.printf_P(PSTR("throttle kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD - Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED - Serial.printf_P(PSTR("traverse: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - - //Extras - #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO - Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_PMAX - Serial.printf_P(PSTR("Y6: %d\n"), (g.rc_6.control_in * 2)); - - #elif CHANNEL_6_TUNING == CH6_RELAY - Serial.printf_P(PSTR(" %d\n"), (g.rc_6.control_in )); - - #endif + tuning(); + Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value); if(Serial.available() > 0){ return (0); @@ -1009,10 +958,12 @@ test_mission(uint8_t argc, const Menu::arg *argv) return (0); } */ + static void print_hit_enter() { Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); } + /* static void fake_out_gps() { From 97d69906dd042635f0f07019512ae4234279a84f Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 10 Sep 2011 15:28:55 -0700 Subject: [PATCH 22/74] Incremented to 2.0.41 --- ArduCopter/system.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index c2cf31502e..3b8ef34edd 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "ArduCopter 2.0.40 Beta", main_menu_commands); +MENU(main_menu, "ArduCopter 2.0.41 Beta", main_menu_commands); #endif // CLI_ENABLED From 8bf4791aba8d269b9bffc00a8276f766b37c2a46 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 11 Sep 2011 06:51:12 +0800 Subject: [PATCH 23/74] firmware build --- .../Firmware/AC2-Hexa-1280.build.log | 21 +++-- .../Firmware/AC2-Hexa-1280.size.txt | 24 +++--- .../Firmware/AC2-Hexa-2560.build.log | 21 +++-- .../Firmware/AC2-Hexa-2560.size.txt | 24 +++--- .../Firmware/AC2-QUADHIL-1280.build.log | 31 ++++---- .../Firmware/AC2-QUADHIL-1280.size.txt | 24 +++--- .../Firmware/AC2-QUADHIL-2560.build.log | 31 ++++---- .../Firmware/AC2-QUADHIL-2560.size.txt | 24 +++--- .../Firmware/AC2-Quad-1280.build.log | 21 +++-- .../Firmware/AC2-Quad-1280.size.txt | 24 +++--- .../Firmware/AC2-Quad-2560.build.log | 21 +++-- .../Firmware/AC2-Quad-2560.size.txt | 24 +++--- .../Firmware/AC2-Tri-1280.build.log | 21 +++-- .../Firmware/AC2-Tri-1280.size.txt | 24 +++--- .../Firmware/AC2-Tri-2560.build.log | 21 +++-- .../Firmware/AC2-Tri-2560.size.txt | 24 +++--- .../Firmware/AC2-Y6-1280.build.log | 21 +++-- .../Firmware/AC2-Y6-1280.size.txt | 24 +++--- .../Firmware/AC2-Y6-2560.build.log | 21 +++-- .../Firmware/AC2-Y6-2560.size.txt | 24 +++--- .../Firmware/firmware2.xml | 12 +-- Tools/ArdupilotMegaPlanner/Firmware/git.log | 76 ++++++++++++++++++- 22 files changed, 341 insertions(+), 217 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt index d8f061f4bc..32abac3437 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 00000090 t dump_log(unsigned char, Menu::arg const*) +00000092 t test_tuning(unsigned char, Menu::arg const*) +00000092 t report_tuning() 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,12 +554,12 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() 000000de t test_adc(unsigned char, Menu::arg const*) +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00002000 T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001fb8 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt index d6992b33c3..0eec1008c4 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 0000008e t dump_log(unsigned char, Menu::arg const*) +00000090 t report_tuning() +00000092 t test_tuning(unsigned char, Menu::arg const*) 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,12 +554,12 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000dc t test_adc(unsigned char, Menu::arg const*) 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001ffe T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001fb6 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log index f9ac117a10..b1aee4e1a4 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -34,8 +33,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined autogenerated:270: warning: 'void ReadSCP1000()' declared 'static' but never defined @@ -49,19 +48,19 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:353: warning: 'old_altitude' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:371: warning: 'abs_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:372: warning: 'ground_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:373: warning: 'ground_temperature' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:378: warning: 'baro_alt' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:354: warning: 'old_altitude' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:372: warning: 'abs_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:373: warning: 'ground_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:374: warning: 'ground_temperature' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:379: warning: 'baro_alt' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used /root/apm/ardupilot-mega/ArduCopter/test.pde:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:24: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined %% libraries/APM_BMP085/APM_BMP085.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt index 49251a6ae6..460519974d 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt @@ -132,7 +132,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -163,6 +162,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -170,6 +170,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -182,6 +183,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -219,6 +221,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -253,7 +256,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -282,6 +284,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -383,6 +386,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status 00000019 r GCS_MAVLINK::update()::__c @@ -480,7 +484,6 @@ 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c 00000057 B dcm -00000058 t test_tuning(unsigned char, Menu::arg const*) 0000005a t read_control_switch() 0000005c t get_num_logs() 0000005c t setup_esc(unsigned char, Menu::arg const*) @@ -510,6 +513,8 @@ 0000008c t setup_frame(unsigned char, Menu::arg const*) 00000090 t init_compass() 00000090 t dump_log(unsigned char, Menu::arg const*) +00000092 t test_tuning(unsigned char, Menu::arg const*) +00000092 t report_tuning() 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -539,10 +544,10 @@ 000000c6 t test_tri(unsigned char, Menu::arg const*) 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -583,18 +588,19 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000029e t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000384 t print_log_menu() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char -00000568 t __static_initialization_and_destruction_0(int, int) +00000570 t __static_initialization_and_destruction_0(int, int) 000005ea T update_roll_pitch_mode() 00000616 t init_ardupilot() -000007c1 b g -000007d6 W Parameters::Parameters() +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() -000011be t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001196 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) 00001494 T GCS_MAVLINK::handleMessage(__mavlink_message*) -0000194a T loop +00001902 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log index f9ac117a10..b1aee4e1a4 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -34,8 +33,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined autogenerated:270: warning: 'void ReadSCP1000()' declared 'static' but never defined @@ -49,19 +48,19 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:353: warning: 'old_altitude' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:371: warning: 'abs_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:372: warning: 'ground_pressure' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:373: warning: 'ground_temperature' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:378: warning: 'baro_alt' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:354: warning: 'old_altitude' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:372: warning: 'abs_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:373: warning: 'ground_pressure' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:374: warning: 'ground_temperature' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:379: warning: 'baro_alt' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used /root/apm/ardupilot-mega/ArduCopter/test.pde:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:24: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined %% libraries/APM_BMP085/APM_BMP085.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt index 77b38118ab..b7f17d09f3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt @@ -132,7 +132,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -163,6 +162,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -170,6 +170,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -182,6 +183,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -219,6 +221,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -253,7 +256,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -282,6 +284,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -383,6 +386,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status 00000019 r GCS_MAVLINK::update()::__c @@ -480,7 +484,6 @@ 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c 00000057 B dcm -00000058 t test_tuning(unsigned char, Menu::arg const*) 0000005a t read_control_switch() 0000005c t get_num_logs() 0000005c t setup_esc(unsigned char, Menu::arg const*) @@ -510,6 +513,8 @@ 0000008c t setup_frame(unsigned char, Menu::arg const*) 0000008e t dump_log(unsigned char, Menu::arg const*) 00000090 t init_compass() +00000090 t report_tuning() +00000092 t test_tuning(unsigned char, Menu::arg const*) 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -539,10 +544,10 @@ 000000c6 t test_tri(unsigned char, Menu::arg const*) 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -583,18 +588,19 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000029e t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000382 t print_log_menu() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char -00000568 t __static_initialization_and_destruction_0(int, int) +00000570 t __static_initialization_and_destruction_0(int, int) 000005ea T update_roll_pitch_mode() 00000616 t init_ardupilot() -000007c1 b g -000007d6 W Parameters::Parameters() +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() -000011be t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001196 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) 00001494 T GCS_MAVLINK::handleMessage(__mavlink_message*) -00001948 T loop +00001900 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt index d880bfc7ae..0438f5d10f 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 00000090 t dump_log(unsigned char, Menu::arg const*) +00000092 t test_tuning(unsigned char, Menu::arg const*) +00000092 t report_tuning() 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,12 +554,12 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() 000000de t test_adc(unsigned char, Menu::arg const*) +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001ece T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001e86 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt index d06cc7853f..b91f764e5a 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 0000008e t dump_log(unsigned char, Menu::arg const*) +00000090 t report_tuning() +00000092 t test_tuning(unsigned char, Menu::arg const*) 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,12 +554,12 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000dc t test_adc(unsigned char, Menu::arg const*) 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001ecc T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001e84 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt index 0ab5cc71b9..2e43c40bbf 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 00000090 t dump_log(unsigned char, Menu::arg const*) +00000092 t test_tuning(unsigned char, Menu::arg const*) +00000092 t report_tuning() 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,13 +554,13 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t setup_motors(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() 000000de t test_adc(unsigned char, Menu::arg const*) +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001e2e T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001de6 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt index 28a12d499f..6eb9d0dfc9 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c 0000000a r Log_Read_Startup()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 0000008e t dump_log(unsigned char, Menu::arg const*) +00000090 t report_tuning() +00000092 t test_tuning(unsigned char, Menu::arg const*) 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,13 +554,13 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t setup_motors(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000dc t test_adc(unsigned char, Menu::arg const*) 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001e2c T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001de4 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt index b765d03870..4d68a192b0 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r report_frame()::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 00000090 t dump_log(unsigned char, Menu::arg const*) +00000092 t test_tuning(unsigned char, Menu::arg const*) +00000092 t report_tuning() 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,12 +554,12 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() 000000de t test_adc(unsigned char, Menu::arg const*) +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001f0e T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001ec6 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log index 6d390dc9bd..ff07e834d3 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log @@ -3,7 +3,6 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1398: warning: 'void tuning()' defined but not used autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -31,8 +30,8 @@ autogenerated:250: warning: 'void calc_altitude_smoothing_error()' declared 'sta /root/apm/ardupilot-mega/ArduCopter/navigation.pde:217: warning: 'void reset_crosstrack()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:222: warning: 'long int get_altitude_above_home()' defined but not used /root/apm/ardupilot-mega/ArduCopter/navigation.pde:243: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:155: warning: 'void throttle_failsafe(uint16_t)' defined but not used -/root/apm/ardupilot-mega/ArduCopter/radio.pde:212: warning: 'void trim_yaw()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:131: warning: 'void throttle_failsafe(uint16_t)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/radio.pde:188: warning: 'void trim_yaw()' defined but not used autogenerated:268: warning: 'void readCommands()' declared 'static' but never defined autogenerated:269: warning: 'void parseCommand(char*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used @@ -44,14 +43,14 @@ autogenerated:287: warning: 'void report_gyro()' declared 'static' but never def autogenerated:295: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined autogenerated:296: warning: 'int read_num_from_serial()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/system.pde:465: warning: 'void set_failsafe(boolean)' defined but not used -autogenerated:310: warning: 'void init_optflow()' declared 'static' but never defined -autogenerated:318: warning: 'void fake_out_gps()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/test.pde:1040: warning: 'void print_motor_out()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:442: warning: 'undo_event' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:448: warning: 'condition_rate' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:462: warning: 'simple_WP' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:467: warning: 'optflow_offset' defined but not used -/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'new_location' defined but not used +autogenerated:311: warning: 'void init_optflow()' declared 'static' but never defined +autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never defined +/root/apm/ardupilot-mega/ArduCopter/test.pde:991: warning: 'void print_motor_out()' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:443: warning: 'undo_event' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:449: warning: 'condition_rate' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:463: warning: 'simple_WP' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:468: warning: 'optflow_offset' defined but not used +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used %% libraries/APM_BMP085/APM_BMP085.o %% libraries/APM_PI/APM_PI.o %% libraries/APM_RC/APM_RC.o diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt index a70917bb1a..7b2bb0fd0e 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt @@ -134,7 +134,6 @@ 00000004 b command_yaw_start_time 00000004 b initial_simple_bearing 00000004 d G_Dt -00000004 b load 00000004 b dTnav 00000004 b nav_lat 00000004 b nav_lon @@ -166,6 +165,7 @@ 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c +00000004 B tuning_value 00000005 r __menu_name__test_menu 00000005 r report_imu()::__c 00000005 r select_logs(unsigned char, Menu::arg const*)::__c @@ -173,6 +173,7 @@ 00000005 r select_logs(unsigned char, Menu::arg const*)::__c 00000005 r Log_Read_Raw()::__c 00000005 r Log_Read_Mode()::__c +00000005 r report_tuning()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c 00000005 r print_log_menu()::__c @@ -186,6 +187,7 @@ 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c 00000005 V Parameters::Parameters()::__c +00000005 V Parameters::Parameters()::__c 00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c @@ -222,6 +224,7 @@ 00000008 r report_frame()::__c 00000008 r report_frame()::__c 00000008 r report_frame()::__c +00000008 r report_tuning()::__c 00000008 r print_log_menu()::__c 00000008 r report_batt_monitor()::__c 00000008 r report_batt_monitor()::__c @@ -256,7 +259,6 @@ 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r test_tuning(unsigned char, Menu::arg const*)::__c 0000000a r report_frame()::__c 0000000a r start_new_log()::__c 0000000a r print_log_menu()::__c @@ -284,6 +286,7 @@ 0000000c V Parameters::Parameters()::__c 0000000d r verify_RTL()::__c 0000000d r select_logs(unsigned char, Menu::arg const*)::__c +0000000d r test_tuning(unsigned char, Menu::arg const*)::__c 0000000d r test_battery(unsigned char, Menu::arg const*)::__c 0000000d r startup_ground()::__c 0000000d r test_wp(unsigned char, Menu::arg const*)::__c @@ -384,6 +387,7 @@ 00000016 r GCS_MAVLINK::update()::__c 00000016 B sonar 00000017 r __menu_name__main_menu +00000018 t setup_tune(unsigned char, Menu::arg const*) 00000018 t setup_accel(unsigned char, Menu::arg const*) 00000018 W AP_VarT::serialize(void*, unsigned int) const 00000018 b mavlink_get_channel_status::m_mavlink_status @@ -485,7 +489,6 @@ 00000056 t readSwitch() 00000056 t dancing_light() 00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000058 t test_tuning(unsigned char, Menu::arg const*) 00000058 t Log_Write_Attitude() 0000005a t read_control_switch() 0000005c t get_num_logs() @@ -517,6 +520,8 @@ 0000008c t print_gyro_offsets() 0000008c t print_accel_offsets() 0000008e t dump_log(unsigned char, Menu::arg const*) +00000090 t report_tuning() +00000092 t test_tuning(unsigned char, Menu::arg const*) 00000095 r init_ardupilot()::__c 00000096 t map_baudrate(signed char, unsigned long) 00000096 t print_wp(Location*, unsigned char) @@ -549,12 +554,12 @@ 000000ca t init_barometer() 000000cc t read_radio() 000000d0 t get_bearing(Location*, Location*) -000000d0 r setup_menu_commands 000000d8 t test_radio(unsigned char, Menu::arg const*) 000000d8 t read_barometer() 000000dc t test_adc(unsigned char, Menu::arg const*) 000000de t Log_Read_Performance() 000000de t Log_Read_Control_Tuning() +000000e0 r setup_menu_commands 000000e0 b mavlink_parse_char::m_mavlink_message 000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) 000000e4 t Log_Read_Optflow() @@ -595,6 +600,7 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) +000002a8 t tuning() 0000031e t read_battery() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -603,10 +609,10 @@ 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -00000798 t __static_initialization_and_destruction_0(int, int) -000007c1 b g -000007d6 W Parameters::Parameters() +000007a0 t __static_initialization_and_destruction_0(int, int) +000007cd b g +000007e8 W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) -000017d0 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001f0c T loop +000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) +00001ec4 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml index 4f556adf34..b285a250b5 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml +++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml @@ -58,7 +58,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex - ArduCopter 2.0.40 Beta Quad + ArduCopter 2.0.41 Beta Quad #define FRAME_CONFIG QUAD_FRAME #define FRAME_ORIENTATION X_FRAME @@ -69,7 +69,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex - ArduCopter 2.0.40 Beta Tri + ArduCopter 2.0.41 Beta Tri #define FRAME_CONFIG TRI_FRAME #define FRAME_ORIENTATION X_FRAME @@ -80,7 +80,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex - ArduCopter 2.0.40 Beta Hexa + ArduCopter 2.0.41 Beta Hexa #define FRAME_CONFIG HEXA_FRAME #define FRAME_ORIENTATION X_FRAME @@ -91,7 +91,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex - ArduCopter 2.0.40 Beta Y6 + ArduCopter 2.0.41 Beta Y6 #define FRAME_CONFIG Y6_FRAME #define FRAME_ORIENTATION X_FRAME @@ -102,7 +102,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.hex - ArduCopter 2.0.40 Beta Heli (2560 only) + ArduCopter 2.0.41 Beta Heli (2560 only) #define AUTO_RESET_LOITER 0 #define FRAME_CONFIG HELI_FRAME @@ -149,7 +149,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex - ArduCopter 2.0.40 Beta Quad Hil + ArduCopter 2.0.41 Beta Quad Hil #define FRAME_CONFIG QUAD_FRAME #define FRAME_ORIENTATION PLUS_FRAME diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index 2a7bfed917..46d7a63a89 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1 +1,75 @@ -Already up-to-date. +From https://code.google.com/p/ardupilot-mega + 431bdb4..22593cc APM_Camera -> origin/APM_Camera + 9a7bcd1..40001f4 master -> origin/master +Updating 9a7bcd1..40001f4 +Fast-forward + ArduCopter/ArduCopter.pde | 108 +- + ArduCopter/Attitude.pde | 2 +- + ArduCopter/Mavlink_Common.h | 12 +- + ArduCopter/Parameters.h | 4 + + ArduCopter/config.h | 4 +- + ArduCopter/defines.h | 19 +- + ArduCopter/radio.pde | 24 - + ArduCopter/setup.pde | 23 + + ArduCopter/system.pde | 2 +- + ArduCopter/test.pde | 57 +- + Tools/ArdupilotMegaPlanner/.gitignore | 4 + + Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj | 1 + + .../ArdupilotMegaPlanner/ArdupilotMega.csproj.user | 13 - + Tools/ArdupilotMegaPlanner/Common.cs | 2 +- + Tools/ArdupilotMegaPlanner/CurrentState.cs | 2 +- + .../ArdupilotMegaPlanner/GCSViews/Configuration.cs | 4 +- + Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs | 440 +---- + Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs | 2 +- + .../ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs | 10 +- + Tools/ArdupilotMegaPlanner/HUD.cs | 50 +- + Tools/ArdupilotMegaPlanner/LogBrowse.cs | 4 +- + Tools/ArdupilotMegaPlanner/MAVLink.cs | 2 +- + Tools/ArdupilotMegaPlanner/MainV2.cs | 25 +- + .../Properties/AssemblyInfo.cs | 2 +- + Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 8 +- + Tools/ArdupilotMegaPlanner/bin/Release/.gitignore | 3 + + .../bin/Release/ArdupilotMegaPlanner.exe | Bin 2052096 -> 2045440 bytes + Tools/ArdupilotMegaPlanner/bin/Release/Updater.exe | Bin 8192 -> 8192 bytes + Tools/ArdupilotMegaPlanner/bin/Release/resedit.exe | Bin 13312 -> 13312 bytes + .../zh-Hans/ArdupilotMegaPlanner.resources.dll | Bin 397312 -> 397312 bytes + Tools/PPMEncoder/ap_ppm_encoder.aps | 1 + + Tools/PPMEncoder/ap_ppm_encoder.aws | 1 + + Tools/PPMEncoder/ap_ppm_encoder.c | 1287 ++++++++++ + Tools/PPMEncoder/default/Makefile | 77 + + Tools/PPMEncoder/default/ap_ppm_encoder.eep | 5 + + Tools/PPMEncoder/default/ap_ppm_encoder.elf | Bin 0 -> 16459 bytes + Tools/PPMEncoder/default/ap_ppm_encoder.hex | 197 ++ + Tools/PPMEncoder/default/ap_ppm_encoder.lss | 2636 ++++++++++++++++++++ + Tools/PPMEncoder/default/ap_ppm_encoder.map | 419 ++++ + Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d | 46 + + Tools/PPMEncoder/default/ppm_enconder_at328.bat | 17 + + Tools/PPMEncoder/servo2ppm_settings.h | 128 + + images/GIT2.PNG | Bin 29001 -> 0 bytes + libraries/AP_Camera/Camera.cpp | 277 -- + libraries/AP_Camera/Camera.h | 71 - + libraries/AP_Mount/AP_Mount.cpp | 184 -- + libraries/AP_Mount/AP_Mount.h | 99 - + libraries/AP_Mount/keywords.txt | 9 - + 48 files changed, 4995 insertions(+), 1286 deletions(-) + create mode 100644 Tools/ArdupilotMegaPlanner/.gitignore + delete mode 100644 Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user + create mode 100644 Tools/ArdupilotMegaPlanner/bin/Release/.gitignore + create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.aps + create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.aws + create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.c + create mode 100644 Tools/PPMEncoder/default/Makefile + create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.eep + create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.elf + create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.hex + create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.lss + create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.map + create mode 100644 Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d + create mode 100644 Tools/PPMEncoder/default/ppm_enconder_at328.bat + create mode 100644 Tools/PPMEncoder/servo2ppm_settings.h + delete mode 100644 images/GIT2.PNG + delete mode 100644 libraries/AP_Camera/Camera.cpp + delete mode 100644 libraries/AP_Camera/Camera.h + delete mode 100644 libraries/AP_Mount/AP_Mount.cpp + delete mode 100644 libraries/AP_Mount/AP_Mount.h + delete mode 100644 libraries/AP_Mount/keywords.txt From f7b7a361a6471cf6f592e1c1fa38cb3b3e38caa9 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 11 Sep 2011 07:11:57 +0800 Subject: [PATCH 24/74] firmware build --- Tools/ArdupilotMegaPlanner/Firmware/git.log | 76 +-------------------- 1 file changed, 1 insertion(+), 75 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index 46d7a63a89..2a7bfed917 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1,75 +1 @@ -From https://code.google.com/p/ardupilot-mega - 431bdb4..22593cc APM_Camera -> origin/APM_Camera - 9a7bcd1..40001f4 master -> origin/master -Updating 9a7bcd1..40001f4 -Fast-forward - ArduCopter/ArduCopter.pde | 108 +- - ArduCopter/Attitude.pde | 2 +- - ArduCopter/Mavlink_Common.h | 12 +- - ArduCopter/Parameters.h | 4 + - ArduCopter/config.h | 4 +- - ArduCopter/defines.h | 19 +- - ArduCopter/radio.pde | 24 - - ArduCopter/setup.pde | 23 + - ArduCopter/system.pde | 2 +- - ArduCopter/test.pde | 57 +- - Tools/ArdupilotMegaPlanner/.gitignore | 4 + - Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj | 1 + - .../ArdupilotMegaPlanner/ArdupilotMega.csproj.user | 13 - - Tools/ArdupilotMegaPlanner/Common.cs | 2 +- - Tools/ArdupilotMegaPlanner/CurrentState.cs | 2 +- - .../ArdupilotMegaPlanner/GCSViews/Configuration.cs | 4 +- - Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs | 440 +---- - Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs | 2 +- - .../ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs | 10 +- - Tools/ArdupilotMegaPlanner/HUD.cs | 50 +- - Tools/ArdupilotMegaPlanner/LogBrowse.cs | 4 +- - Tools/ArdupilotMegaPlanner/MAVLink.cs | 2 +- - Tools/ArdupilotMegaPlanner/MainV2.cs | 25 +- - .../Properties/AssemblyInfo.cs | 2 +- - Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 8 +- - Tools/ArdupilotMegaPlanner/bin/Release/.gitignore | 3 + - .../bin/Release/ArdupilotMegaPlanner.exe | Bin 2052096 -> 2045440 bytes - Tools/ArdupilotMegaPlanner/bin/Release/Updater.exe | Bin 8192 -> 8192 bytes - Tools/ArdupilotMegaPlanner/bin/Release/resedit.exe | Bin 13312 -> 13312 bytes - .../zh-Hans/ArdupilotMegaPlanner.resources.dll | Bin 397312 -> 397312 bytes - Tools/PPMEncoder/ap_ppm_encoder.aps | 1 + - Tools/PPMEncoder/ap_ppm_encoder.aws | 1 + - Tools/PPMEncoder/ap_ppm_encoder.c | 1287 ++++++++++ - Tools/PPMEncoder/default/Makefile | 77 + - Tools/PPMEncoder/default/ap_ppm_encoder.eep | 5 + - Tools/PPMEncoder/default/ap_ppm_encoder.elf | Bin 0 -> 16459 bytes - Tools/PPMEncoder/default/ap_ppm_encoder.hex | 197 ++ - Tools/PPMEncoder/default/ap_ppm_encoder.lss | 2636 ++++++++++++++++++++ - Tools/PPMEncoder/default/ap_ppm_encoder.map | 419 ++++ - Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d | 46 + - Tools/PPMEncoder/default/ppm_enconder_at328.bat | 17 + - Tools/PPMEncoder/servo2ppm_settings.h | 128 + - images/GIT2.PNG | Bin 29001 -> 0 bytes - libraries/AP_Camera/Camera.cpp | 277 -- - libraries/AP_Camera/Camera.h | 71 - - libraries/AP_Mount/AP_Mount.cpp | 184 -- - libraries/AP_Mount/AP_Mount.h | 99 - - libraries/AP_Mount/keywords.txt | 9 - - 48 files changed, 4995 insertions(+), 1286 deletions(-) - create mode 100644 Tools/ArdupilotMegaPlanner/.gitignore - delete mode 100644 Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj.user - create mode 100644 Tools/ArdupilotMegaPlanner/bin/Release/.gitignore - create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.aps - create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.aws - create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.c - create mode 100644 Tools/PPMEncoder/default/Makefile - create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.eep - create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.elf - create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.hex - create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.lss - create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.map - create mode 100644 Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d - create mode 100644 Tools/PPMEncoder/default/ppm_enconder_at328.bat - create mode 100644 Tools/PPMEncoder/servo2ppm_settings.h - delete mode 100644 images/GIT2.PNG - delete mode 100644 libraries/AP_Camera/Camera.cpp - delete mode 100644 libraries/AP_Camera/Camera.h - delete mode 100644 libraries/AP_Mount/AP_Mount.cpp - delete mode 100644 libraries/AP_Mount/AP_Mount.h - delete mode 100644 libraries/AP_Mount/keywords.txt +Already up-to-date. From bb1b7b47a77b2a87074372ff92ba17330136d8b1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 10 Sep 2011 16:25:52 -0700 Subject: [PATCH 25/74] Fixed numbering issue with defines --- ArduCopter/defines.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 62339e3b79..6b687b81b2 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -146,15 +146,15 @@ #define CH6_STABILIZE_KI 2 #define CH6_YAW_KP 3 // Rate -#define CH6_RATE_KP 5 -#define CH6_RATE_KI 6 -#define CH6_YAW_RATE_KP 7 +#define CH6_RATE_KP 4 +#define CH6_RATE_KI 5 +#define CH6_YAW_RATE_KP 6 // Altitude -#define CH6_THROTTLE_KP 8 +#define CH6_THROTTLE_KP 7 // Extras -#define CH6_TOP_BOTTOM_RATIO 9 -#define CH6_RELAY 10 -#define CH6_TRAVERSE_SPEED 11 +#define CH6_TOP_BOTTOM_RATIO 8 +#define CH6_RELAY 9 +#define CH6_TRAVERSE_SPEED 10 // nav byte mask // ------------- From 73b5d1fbbfe2cdab29a9c61c2dd72e4ad8248513 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 11 Sep 2011 07:28:00 +0800 Subject: [PATCH 26/74] firmware build --- Tools/ArdupilotMegaPlanner/Firmware/git.log | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index 2a7bfed917..896bcb6fb2 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1 +1,6 @@ -Already up-to-date. +From https://code.google.com/p/ardupilot-mega + 4e3e372..ebb25b6 master -> origin/master +Updating 4e3e372..ebb25b6 +Fast-forward + ArduCopter/defines.h | 14 +++++++------- + 1 files changed, 7 insertions(+), 7 deletions(-) From d06329d4b949f153f521d815e33b066bc63f7bac Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 11 Sep 2011 02:14:58 +0200 Subject: [PATCH 27/74] Signed-off-by: Olivier ADLER --- Tools/ArduCoder/test.txt | 1 + 1 file changed, 1 insertion(+) create mode 100644 Tools/ArduCoder/test.txt diff --git a/Tools/ArduCoder/test.txt b/Tools/ArduCoder/test.txt new file mode 100644 index 0000000000..9daeafb986 --- /dev/null +++ b/Tools/ArduCoder/test.txt @@ -0,0 +1 @@ +test From d84f80cb0d44b94621899c53bea706d043cbf62c Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:34:47 +0200 Subject: [PATCH 28/74] Moved enum values around to make code simpler to read and use --- libraries/RC_Channel/RC_Channel.cpp | 14 +++++++------- libraries/RC_Channel/RC_Channel.h | 26 +++++++++++++------------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 2673f26e53..c1be664e2b 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -264,21 +264,21 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) case k_none: // disabled return; break; + case k_manual: // manual + radio_out = radio_in; + break; + case k_flap: // flaps + case k_flap_auto: // flaps automated + case k_aileron: // aileron + case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) case k_mount_yaw: // mount yaw (pan) case k_mount_pitch: // mount pitch (tilt) case k_mount_roll: // mount roll case k_cam_trigger: // camera trigger case k_cam_open: // camera open - case k_flap: // flaps - case k_flap_auto: // flaps automated - case k_aileron: // aileron - case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) case k_egg_drop: // egg drop case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning break; - case k_manual: // manual - radio_out = radio_in; - break; } APM_RC.OutputCh(ch_nr, radio_out); diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 2cc4df73d3..6f8b3c6d17 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -123,18 +123,18 @@ public: typedef enum { - k_none = 0, // 0=disabled - k_mount_yaw = 1, // 1=mount yaw (pan) - k_mount_pitch = 2, // 2=mount pitch (tilt) - k_mount_roll = 3, // 3=mount roll - k_cam_trigger = 4, // 4=camera trigger - k_cam_open = 5, // 5=camera open - k_flap = 6, // 6=flap - k_flap_auto = 7, // 7=flap automated - k_aileron = 8, // 8=aileron - k_flaperon = 9, // 9=flaperon (flaps and aileron combined, needs two independent servos one for each wing) - k_egg_drop = 10, // 10=egg drop - k_manual = 11, // 11=manual, just pass-thru the RC in signal + k_none = 0, // disabled + k_manual = 1, // manual, just pass-thru the RC in signal + k_flap = 2, // flap + k_flap_auto = 3, // flap automated + k_aileron = 4, // aileron + k_flaperon = 5, // flaperon (flaps and aileron combined, needs two independent servos one for each wing) + k_mount_yaw = 6, // mount yaw (pan) + k_mount_pitch = 7, // mount pitch (tilt) + k_mount_roll = 8, // mount roll + k_cam_trigger = 9, // camera trigger + k_cam_open = 10, // camera open + k_egg_drop = 11, // egg drop k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; @@ -142,7 +142,7 @@ public: // It would probably save some memory. But it can only be added to enums and not to typedefs :( AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func - AP_Aux_srv_func function; // 0=disabled, 1=mount yaw (pan), 2=mount pitch (tilt), 3=mount roll, 4=camera trigger, 5=camera open, 6=flap, 7=flap auto, 8=aileron, 9=flaperon, 10=egg drop, 11=manual + AP_Aux_srv_func function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units From 84f108d598fe29868deb9290c68ca1156745f40e Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:36:39 +0200 Subject: [PATCH 29/74] Fix manual values mapping --- libraries/AP_Mount/AP_Mount.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 5629e0ee3e..6506e5f046 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -124,31 +124,27 @@ void AP_Mount::update_mount() yaw_angle=100*_roam_angles.z; break; } - case k_none: - { - //do nothing - break; - } case k_manual: // radio manual control if (g_rc_function[RC_Channel_aux::k_mount_roll]) roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in, g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min, g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max, g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max); + g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max); if (g_rc_function[RC_Channel_aux::k_mount_pitch]) pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in, g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min, g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max, g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max); + g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max); if (g_rc_function[RC_Channel_aux::k_mount_yaw]) yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in, g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min, g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max, g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max); + g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max); break; + case k_none: default: { //do nothing From 8aeee578b4524f50ef4b7c67a9e642dfd677fe34 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:40:13 +0200 Subject: [PATCH 30/74] For now point the camera manually via the RC inputs --- ArduPlane/ArduPlane.pde | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 45b95b69ee..e99afc1db1 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -577,7 +577,13 @@ static void medium_loop() { #if CAMERA == ENABLED // TODO replace home with a POI coming from a MavLink message or command - camera_mount.set_GPS_target(home); + //camera_mount.set_GPS_target(home); + + // For now point the camera manually via the RC inputs (later remove these two lines) + // for simple dcm tests, replace k_manual with k_stabilise + camera_mount.set_mode(AP_Mount::k_manual); + camera_mount.update_mount(); + g.camera.trigger_pic_cleanup(); #endif From dd19a7302dfcb135cf22d4f984f932bd60bc5d6d Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:41:48 +0200 Subject: [PATCH 31/74] I do not understand this, but it looks like it needs int8 here --- libraries/RC_Channel/RC_Channel.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 6f8b3c6d17..55dec9305d 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -140,9 +140,10 @@ public: // TODO It would be great if the "packed" attribute could be added to this somehow // It would probably save some memory. But it can only be added to enums and not to typedefs :( - AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func + /*AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func - AP_Aux_srv_func function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop + AP_Aux_srv_func function;*/ // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop + AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units From b4a51806bcb420c39b34cac88744e7d2fbfc07b5 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 10 Sep 2011 18:37:28 -0700 Subject: [PATCH 32/74] Added Nav Rate tuning --- ArduCopter/ArduCopter.pde | 6 ++++++ ArduCopter/defines.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 423c1cfd7f..37331f0ce3 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1454,6 +1454,12 @@ static void tuning(){ g.rc_6.set_range(0,1000); g.waypoint_speed_max = g.rc_6.control_in; break; + + case CH6_NAV_P: + g.rc_6.set_range(0,6000); + g.pi_nav_lat.kP(tuning_value); + g.pi_nav_lon.kP(tuning_value); + break; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6b687b81b2..848d2e79c3 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -156,6 +156,8 @@ #define CH6_RELAY 9 #define CH6_TRAVERSE_SPEED 10 +#define CH6_NAV_P 11 + // nav byte mask // ------------- #define NAV_LOCATION 1 From 482844b87a20ca997af7d74d0d806d057772d458 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 10 Sep 2011 18:44:56 -0700 Subject: [PATCH 33/74] Added Input Voltage value to params --- ArduCopter/Parameters.h | 3 +++ ArduCopter/defines.h | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 9013557147..c2680f856e 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -77,6 +77,7 @@ public: k_param_frame_orientation, k_param_top_bottom_ratio, k_param_optflow_enabled, + k_param_input_voltage, // // 160: Navigation parameters @@ -237,6 +238,7 @@ public: AP_Int8 frame_orientation; AP_Float top_bottom_ratio; AP_Int8 optflow_enabled; + AP_Float input_voltage; #if FRAME_CONFIG == HELI_FRAME // Heli @@ -299,6 +301,7 @@ public: pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")), + input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("IN_VOLT")), waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")), waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 848d2e79c3..5a0dcbfd91 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -313,8 +313,8 @@ #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_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT +#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*VOLT_DIV_RATIO +#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT //#define BARO_FILTER_SIZE 8 /* ************************************************************** */ From 82596721619d07a274d4d9e3ffbd93ca37658ee9 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 11 Sep 2011 09:47:03 +0800 Subject: [PATCH 34/74] firmware build --- .../Firmware/AC2-Hexa-1280.size.txt | 11 ++++++----- .../Firmware/AC2-Hexa-2560.size.txt | 11 ++++++----- .../Firmware/AC2-QUADHIL-1280.size.txt | 11 ++++++----- .../Firmware/AC2-QUADHIL-2560.size.txt | 11 ++++++----- .../Firmware/AC2-Quad-1280.size.txt | 11 ++++++----- .../Firmware/AC2-Quad-2560.size.txt | 11 ++++++----- .../Firmware/AC2-Tri-1280.size.txt | 11 ++++++----- .../Firmware/AC2-Tri-2560.size.txt | 11 ++++++----- .../Firmware/AC2-Y6-1280.size.txt | 11 ++++++----- .../Firmware/AC2-Y6-2560.size.txt | 11 ++++++----- Tools/ArdupilotMegaPlanner/Firmware/git.log | 11 +++++++---- 11 files changed, 67 insertions(+), 54 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt index 32abac3437..ee9cf57691 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000384 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt index 0eec1008c4..84dec1b6a0 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000382 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt index 460519974d..300eeeb23c 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt @@ -230,6 +230,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -588,18 +589,18 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000029e t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000384 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char -00000570 t __static_initialization_and_destruction_0(int, int) +00000578 t __static_initialization_and_destruction_0(int, int) 000005ea T update_roll_pitch_mode() 00000616 t init_ardupilot() -000007cd b g -000007e8 W Parameters::Parameters() +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 00001196 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) 00001494 T GCS_MAVLINK::handleMessage(__mavlink_message*) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt index b7f17d09f3..9781e6a0e6 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt @@ -230,6 +230,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -588,18 +589,18 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 0000029e t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000382 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char -00000570 t __static_initialization_and_destruction_0(int, int) +00000578 t __static_initialization_and_destruction_0(int, int) 000005ea T update_roll_pitch_mode() 00000616 t init_ardupilot() -000007cd b g -000007e8 W Parameters::Parameters() +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 00001196 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) 00001494 T GCS_MAVLINK::handleMessage(__mavlink_message*) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt index 0438f5d10f..a48a9b82be 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000384 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt index b91f764e5a..b8ece535cd 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000382 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt index 2e43c40bbf..02d799cbcd 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000384 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt index 6eb9d0dfc9..e093161912 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000382 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt index 4d68a192b0..5d308cf013 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000384 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt index 7b2bb0fd0e..98b0c692ae 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt @@ -233,6 +233,7 @@ 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c 00000008 V Parameters::Parameters()::__c +00000008 V Parameters::Parameters()::__c 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c 00000009 r print_switch(unsigned char, unsigned char)::__c 00000009 r print_log_menu()::__c @@ -600,18 +601,18 @@ 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 00000286 t test_imu(unsigned char, Menu::arg const*) -000002a8 t tuning() -0000031e t read_battery() +000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() 00000382 t print_log_menu() +000003be t read_battery() 000003dc T update_yaw_mode() 000004b2 t mavlink_parse_char 000005ea T update_roll_pitch_mode() 0000062e t init_ardupilot() -000007a0 t __static_initialization_and_destruction_0(int, int) -000007cd b g -000007e8 W Parameters::Parameters() +000007a8 t __static_initialization_and_destruction_0(int, int) +000007dc b g +0000083c W Parameters::Parameters() 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index 896bcb6fb2..d43656d340 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1,6 +1,9 @@ From https://code.google.com/p/ardupilot-mega - 4e3e372..ebb25b6 master -> origin/master -Updating 4e3e372..ebb25b6 + 22593cc..5e95582 APM_Camera -> origin/APM_Camera + 0448983..dc0031c master -> origin/master +Updating 0448983..dc0031c Fast-forward - ArduCopter/defines.h | 14 +++++++------- - 1 files changed, 7 insertions(+), 7 deletions(-) + ArduCopter/ArduCopter.pde | 6 ++++++ + ArduCopter/Parameters.h | 3 +++ + ArduCopter/defines.h | 6 ++++-- + 3 files changed, 13 insertions(+), 2 deletions(-) From 51a8ec0ba8af9f5823c5faf4eae9c50531fd555e Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 04:04:02 +0200 Subject: [PATCH 35/74] Fix RC range reversal --- ArduPlane/APM_Config.h~ | 35 +++++++++++++++++++++++++++++++++++ ArduPlane/ArduPlane.pde | 4 ++-- ArduPlane/Parameters.h | 2 +- ArduPlane/serialsent.raw | Bin 0 -> 390 bytes ArduPlane/upload1 | 2 ++ 5 files changed, 40 insertions(+), 3 deletions(-) create mode 100644 ArduPlane/APM_Config.h~ create mode 100644 ArduPlane/serialsent.raw create mode 100755 ArduPlane/upload1 diff --git a/ArduPlane/APM_Config.h~ b/ArduPlane/APM_Config.h~ new file mode 100644 index 0000000000..4ebb342fd1 --- /dev/null +++ b/ArduPlane/APM_Config.h~ @@ -0,0 +1,35 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from +// their default values, place the appropriate #define statements here. + +// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented) +//#define SERIAL3_BAUD 38400 +//#define GCS_PROTOCOL GCS_PROTOCOL_NONE + + +// You may also put an include statement here to point at another configuration file. This is convenient if you maintain +// different configuration files for different aircraft or HIL simulation. See the examples below +//#include "APM_Config_mavlink_hil.h" +//#include "Skywalker.h" + +// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable: + +/* +#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK +#define HIL_MODE HIL_MODE_ATTITUDE +#define HIL_PORT 0 +#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK +#define GCS_PORT 3 +*/ + + +// ----- Camera definitions ------ +// ------------------------------ +#define CAMERA ENABLED +#define CAM_DEBUG DISABLED + +// - Options to reduce code size - +// ------------------------------- +// Disable text based terminal configuration +#define CLI_ENABLED DISABLED diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index e99afc1db1..f6b39a036c 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.23" +#define THISFIRMWARE "ArduPilotMega V2.24" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -581,7 +581,7 @@ static void medium_loop() // For now point the camera manually via the RC inputs (later remove these two lines) // for simple dcm tests, replace k_manual with k_stabilise - camera_mount.set_mode(AP_Mount::k_manual); + camera_mount.set_mode(AP_Mount::k_stabilise); camera_mount.update_mount(); g.camera.trigger_pic_cleanup(); diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f0d930b1fb..7b62d9a8fa 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 11; + static const uint16_t k_format_version = 12; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) diff --git a/ArduPlane/serialsent.raw b/ArduPlane/serialsent.raw new file mode 100644 index 0000000000000000000000000000000000000000..3df7365eb225e7b20e9ca1741e8f05dd09522656 GIT binary patch literal 390 zcmWG#WBSkN#K_3Rz{r@njw_Un8P4@<0&-cvTs8(~#^>xnE-RdSKpV(q19Q0;xENdI zfn0VtcabrW%K_$c17(kbjN*iIAAv050&^LGCfi;Fa=GDLslPxj517jgl%2@{ Date: Sun, 11 Sep 2011 04:07:36 +0200 Subject: [PATCH 36/74] Revert "Fix RC range reversal" This reverts commit d5046b1097b0b5455aa1a59e9dd80533e589432e. --- ArduPlane/APM_Config.h~ | 35 ----------------------------------- ArduPlane/ArduPlane.pde | 4 ++-- ArduPlane/Parameters.h | 2 +- ArduPlane/serialsent.raw | Bin 390 -> 0 bytes ArduPlane/upload1 | 2 -- 5 files changed, 3 insertions(+), 40 deletions(-) delete mode 100644 ArduPlane/APM_Config.h~ delete mode 100644 ArduPlane/serialsent.raw delete mode 100755 ArduPlane/upload1 diff --git a/ArduPlane/APM_Config.h~ b/ArduPlane/APM_Config.h~ deleted file mode 100644 index 4ebb342fd1..0000000000 --- a/ArduPlane/APM_Config.h~ +++ /dev/null @@ -1,35 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from -// their default values, place the appropriate #define statements here. - -// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented) -//#define SERIAL3_BAUD 38400 -//#define GCS_PROTOCOL GCS_PROTOCOL_NONE - - -// You may also put an include statement here to point at another configuration file. This is convenient if you maintain -// different configuration files for different aircraft or HIL simulation. See the examples below -//#include "APM_Config_mavlink_hil.h" -//#include "Skywalker.h" - -// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable: - -/* -#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK -#define HIL_MODE HIL_MODE_ATTITUDE -#define HIL_PORT 0 -#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK -#define GCS_PORT 3 -*/ - - -// ----- Camera definitions ------ -// ------------------------------ -#define CAMERA ENABLED -#define CAM_DEBUG DISABLED - -// - Options to reduce code size - -// ------------------------------- -// Disable text based terminal configuration -#define CLI_ENABLED DISABLED diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f6b39a036c..e99afc1db1 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.24" +#define THISFIRMWARE "ArduPilotMega V2.23" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -581,7 +581,7 @@ static void medium_loop() // For now point the camera manually via the RC inputs (later remove these two lines) // for simple dcm tests, replace k_manual with k_stabilise - camera_mount.set_mode(AP_Mount::k_stabilise); + camera_mount.set_mode(AP_Mount::k_manual); camera_mount.update_mount(); g.camera.trigger_pic_cleanup(); diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7b62d9a8fa..f0d930b1fb 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 12; + static const uint16_t k_format_version = 11; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) diff --git a/ArduPlane/serialsent.raw b/ArduPlane/serialsent.raw deleted file mode 100644 index 3df7365eb225e7b20e9ca1741e8f05dd09522656..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 390 zcmWG#WBSkN#K_3Rz{r@njw_Un8P4@<0&-cvTs8(~#^>xnE-RdSKpV(q19Q0;xENdI zfn0VtcabrW%K_$c17(kbjN*iIAAv050&^LGCfi;Fa=GDLslPxj517jgl%2@{ Date: Sun, 11 Sep 2011 04:12:04 +0200 Subject: [PATCH 37/74] Fix RC range reversal --- ArduPlane/ArduPlane.pde | 4 ++-- ArduPlane/Parameters.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index e99afc1db1..f6b39a036c 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.23" +#define THISFIRMWARE "ArduPilotMega V2.24" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -581,7 +581,7 @@ static void medium_loop() // For now point the camera manually via the RC inputs (later remove these two lines) // for simple dcm tests, replace k_manual with k_stabilise - camera_mount.set_mode(AP_Mount::k_manual); + camera_mount.set_mode(AP_Mount::k_stabilise); camera_mount.update_mount(); g.camera.trigger_pic_cleanup(); diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f0d930b1fb..7b62d9a8fa 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 11; + static const uint16_t k_format_version = 12; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) From 9f14c5f578e681038b02848514bee4cc9d9eeae4 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 04:12:46 +0200 Subject: [PATCH 38/74] Fix RC range reversal --- libraries/RC_Channel/RC_Channel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index c1be664e2b..7c497c828e 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -121,7 +121,7 @@ RC_Channel::calc_pwm(void) { if(_type == RC_CHANNEL_RANGE){ pwm_out = range_to_pwm(); - radio_out = pwm_out + radio_min; + radio_out = (_reverse >=0 ? pwm_out + radio_min : radio_max - pwm_out); }else if(_type == RC_CHANNEL_ANGLE_RAW){ pwm_out = (float)servo_out * .1; From 93cbe664d691a09ae74253dd5ca11f82f1db0c49 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 17:41:18 +0200 Subject: [PATCH 39/74] Only run camera code if camera support is enabled --- ArduPlane/radio.pde | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index f32dd5e367..4f7292879d 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -23,6 +23,19 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges + if (g_rc_function[RC_Channel_aux::k_flap]) { + g_rc_function[RC_Channel_aux::k_flap]->set_range(0,100); + } + if (g_rc_function[RC_Channel_aux::k_flap_auto]) { + g_rc_function[RC_Channel_aux::k_flap_auto]->set_range(0,100); + } + if (g_rc_function[RC_Channel_aux::k_aileron]) { + g_rc_function[RC_Channel_aux::k_aileron]->set_angle(SERVO_MAX); + } + if (g_rc_function[RC_Channel_aux::k_flaperon]) { + g_rc_function[RC_Channel_aux::k_flaperon]->set_range(0,100); + } +#if CAMERA == ENABLED if (g_rc_function[RC_Channel_aux::k_mount_yaw]) { g_rc_function[RC_Channel_aux::k_mount_yaw]->set_range( g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, @@ -44,22 +57,9 @@ static void init_rc_in() g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); } if (g_rc_function[RC_Channel_aux::k_cam_open]) { - g_rc_function[RC_Channel_aux::k_cam_open]->set_range( - g_rc_function[RC_Channel_aux::k_cam_open]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_cam_open]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_flap]) { - g_rc_function[RC_Channel_aux::k_flap]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_flap_auto]) { - g_rc_function[RC_Channel_aux::k_flap_auto]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_aileron]) { - g_rc_function[RC_Channel_aux::k_aileron]->set_angle(SERVO_MAX); - } - if (g_rc_function[RC_Channel_aux::k_flaperon]) { - g_rc_function[RC_Channel_aux::k_flaperon]->set_range(0,100); + g_rc_function[RC_Channel_aux::k_cam_open]->set_range(0,100); } +#endif if (g_rc_function[RC_Channel_aux::k_egg_drop]) { g_rc_function[RC_Channel_aux::k_egg_drop]->set_range(0,100); } From cfb575b557d1d2bca32a8bba8cd72a06656478c7 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sun, 11 Sep 2011 18:32:00 +0200 Subject: [PATCH 40/74] Signed-off-by: Olivier ADLER --- Tools/ArduCoder/test.txt | 1 - 1 file changed, 1 deletion(-) delete mode 100644 Tools/ArduCoder/test.txt diff --git a/Tools/ArduCoder/test.txt b/Tools/ArduCoder/test.txt deleted file mode 100644 index 9daeafb986..0000000000 --- a/Tools/ArduCoder/test.txt +++ /dev/null @@ -1 +0,0 @@ -test From c5fd79202436fff009470aaaebf00745f74ce6fb Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 18:32:24 +0200 Subject: [PATCH 41/74] Make the switch case complete --- libraries/RC_Channel/RC_Channel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 7c497c828e..6363a108c9 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -278,6 +278,7 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) case k_cam_open: // camera open case k_egg_drop: // egg drop case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning + default: break; } From 53b8cfcf478b1e77fa60d185c1b5385631b28890 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sun, 11 Sep 2011 18:36:09 +0200 Subject: [PATCH 42/74] Signed-off-by: Olivier ADLER --- Tools/ArduPPM/test.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Tools/ArduPPM/test.txt diff --git a/Tools/ArduPPM/test.txt b/Tools/ArduPPM/test.txt new file mode 100644 index 0000000000..e69de29bb2 From d88b3644deaf6df882d8a3d80ba760516eaca1a8 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sun, 11 Sep 2011 18:42:13 +0200 Subject: [PATCH 43/74] Signed-off-by: Olivier ADLER removing test file --- Tools/ArduPPM/test.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 Tools/ArduPPM/test.txt diff --git a/Tools/ArduPPM/test.txt b/Tools/ArduPPM/test.txt deleted file mode 100644 index e69de29bb2..0000000000 From 213969202a5a5086959a7d957a4f4c554bf4d657 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 19:13:01 +0200 Subject: [PATCH 44/74] Moved the RC_Channel_aux class to its own file. The includes could be improved, has anyone got any ideas how ? --- ArduPlane/ArduPlane.pde | 1 + libraries/AP_Camera/AP_Camera.cpp | 2 +- libraries/AP_Mount/AP_Mount.cpp | 4 +- libraries/RC_Channel/RC_Channel.cpp | 66 ----------------------- libraries/RC_Channel/RC_Channel.h | 55 -------------------- libraries/RC_Channel/RC_Channel_aux.cpp | 69 +++++++++++++++++++++++++ libraries/RC_Channel/RC_Channel_aux.h | 56 ++++++++++++++++++++ 7 files changed, 130 insertions(+), 123 deletions(-) create mode 100644 libraries/RC_Channel/RC_Channel_aux.cpp create mode 100644 libraries/RC_Channel/RC_Channel_aux.h diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f6b39a036c..02884aab63 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -39,6 +39,7 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library +#include <../libraries/RC_Channel/RC_Channel_aux.h> // Auxiliary RC Channel Library (Channels 5..8) #include // Range finder library #include #include // Photo or video camera diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index df32a4adac..6fed9b86cb 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- #include -#include +#include <../RC_Channel/RC_Channel_aux.h> extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function extern long wp_distance; diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6506e5f046..6cf76c9351 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1,5 +1,7 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + #include -#include +#include <../RC_Channel/RC_Channel_aux.h> extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 6363a108c9..aa189dff15 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -11,7 +11,6 @@ #include #include -#include #include "WProgram.h" #include "RC_Channel.h" @@ -219,68 +218,3 @@ RC_Channel::norm_output() else return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim); } - -int16_t -RC_Channel_aux::closest_limit(int16_t angle) -{ - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - int16_t min = angle_min / 10; - int16_t max = angle_max / 10; - - // Make sure the angle lies in the interval [-180 .. 180[ degrees - while (angle < -1800) angle += 3600; - while (angle >= 1800) angle -= 3600; - - // Make sure the angle limits lie in the interval [-180 .. 180[ degrees - while (min < -1800) min += 3600; - while (min >= 1800) min -= 3600; - while (max < -1800) max += 3600; - while (max >= 1800) max -= 3600; - set_range(min, max); - - // If the angle is outside servo limits, saturate the angle to the closest limit - // On a circle the closest angular position must be carefully calculated to account for wrap-around - if ((angle < min) && (angle > max)){ - // angle error if min limit is used - int16_t err_min = min - angle + (anglemax?0:3600); // add 360 degrees if on the "wrong side" - angle = err_min -// TODO is this include really necessary ? -#include /// @class RC_Channel /// @brief Object managing one RC channel @@ -104,57 +102,4 @@ class RC_Channel{ int16_t _low; }; -/// @class RC_Channel_aux -/// @brief Object managing one aux. RC channel (CH5-8), with information about its function -/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements -class RC_Channel_aux : public RC_Channel{ -public: - /// Constructor - /// - /// @param key EEPROM storage key for the channel trim parameters. - /// @param name Optional name for the group. - /// - RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) : - RC_Channel(key, name), - function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name - angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection - angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection - {} - - typedef enum - { - k_none = 0, // disabled - k_manual = 1, // manual, just pass-thru the RC in signal - k_flap = 2, // flap - k_flap_auto = 3, // flap automated - k_aileron = 4, // aileron - k_flaperon = 5, // flaperon (flaps and aileron combined, needs two independent servos one for each wing) - k_mount_yaw = 6, // mount yaw (pan) - k_mount_pitch = 7, // mount pitch (tilt) - k_mount_roll = 8, // mount roll - k_cam_trigger = 9, // camera trigger - k_cam_open = 10, // camera open - k_egg_drop = 11, // egg drop - k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) - } Aux_servo_function_t; - - // TODO It would be great if the "packed" attribute could be added to this somehow - // It would probably save some memory. But it can only be added to enums and not to typedefs :( - /*AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func - - AP_Aux_srv_func function;*/ // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop - AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop - AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units - AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units - - int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval - - void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it - -}; - #endif - - - - diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp new file mode 100644 index 0000000000..f6ceb5fed5 --- /dev/null +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -0,0 +1,69 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#include "RC_Channel_aux.h" + +int16_t +RC_Channel_aux::closest_limit(int16_t angle) +{ + // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic + int16_t min = angle_min / 10; + int16_t max = angle_max / 10; + + // Make sure the angle lies in the interval [-180 .. 180[ degrees + while (angle < -1800) angle += 3600; + while (angle >= 1800) angle -= 3600; + + // Make sure the angle limits lie in the interval [-180 .. 180[ degrees + while (min < -1800) min += 3600; + while (min >= 1800) min -= 3600; + while (max < -1800) max += 3600; + while (max >= 1800) max -= 3600; + set_range(min, max); + + // If the angle is outside servo limits, saturate the angle to the closest limit + // On a circle the closest angular position must be carefully calculated to account for wrap-around + if ((angle < min) && (angle > max)){ + // angle error if min limit is used + int16_t err_min = min - angle + (anglemax?0:3600); // add 360 degrees if on the "wrong side" + angle = err_min Date: Sun, 11 Sep 2011 11:03:55 -0700 Subject: [PATCH 45/74] Added a state machine intended to run at 250hz to the DCM. Spread load of DCM out to lower CPU. --- libraries/AP_DCM/AP_DCM.cpp | 71 ++++++++++++++++++++++++++++++++----- libraries/AP_DCM/AP_DCM.h | 7 +++- 2 files changed, 69 insertions(+), 9 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 6e297ca49e..b6dcaaf4b1 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -41,7 +41,7 @@ AP_DCM::set_compass(Compass *compass) /**************************************************/ void -AP_DCM::update_DCM(float _G_Dt) +AP_DCM::update_DCM_fast(float _G_Dt) { _imu->update(); _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors @@ -49,13 +49,48 @@ AP_DCM::update_DCM(float _G_Dt) matrix_update(_G_Dt); // Integrate the DCM matrix - //if(_toggle){ - normalize(); // Normalize the DCM matrix - //}else{ - drift_correction(); // Perform drift correction - //} - //_toggle = !_toggle; + switch(_toggle++){ + case 0: + normalize(); // Normalize the DCM matrix + break; + case 1: + //drift_correction(); // Normalize the DCM matrix + euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation + break; + + case 2: + drift_correction(); // Normalize the DCM matrix + break; + + case 3: + //drift_correction(); // Normalize the DCM matrix + euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation + break; + + case 4: + euler_yaw(); + break; + + default: + euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation + _toggle = 0; + //drift_correction(); // Normalize the DCM matrix + break; + } +} + +/**************************************************/ +void +AP_DCM::update_DCM(float _G_Dt) +{ + _imu->update(); + _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors + _accel_vector = _imu->get_accel(); // Get current values for IMU sensors + + matrix_update(_G_Dt); // Integrate the DCM matrix + normalize(); // Normalize the DCM matrix + drift_correction(); // Perform drift correction euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation } @@ -83,11 +118,12 @@ AP_DCM::matrix_update(float _G_Dt) Matrix3f temp_matrix; //Record when you saturate any of the gyros. + /* if( (fabs(_gyro_vector.x) >= radians(300)) || (fabs(_gyro_vector.y) >= radians(300)) || (fabs(_gyro_vector.z) >= radians(300))){ gyro_sat_count++; - } + }*/ _omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega) _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms @@ -335,6 +371,25 @@ AP_DCM::euler_angles(void) yaw_sensor += 36000; } +void +AP_DCM::euler_rp(void) +{ + pitch = -asin(_dcm_matrix.c.x); + roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z); + roll_sensor = degrees(roll) * 100; + pitch_sensor = degrees(pitch) * 100; +} + +void +AP_DCM::euler_yaw(void) +{ + yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); + yaw_sensor = degrees(yaw) * 100; + + if (yaw_sensor < 0) + yaw_sensor += 36000; +} + /**************************************************/ float diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index bd705da8f8..9a10eb03bc 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -50,6 +50,7 @@ public: // Methods void update_DCM(float _G_Dt); + void update_DCM_fast(float _G_Dt); float get_health(void); @@ -98,6 +99,10 @@ private: void drift_correction(void); void euler_angles(void); + void euler_rp(void); + void euler_yaw(void); + + // members Compass * _compass; @@ -122,7 +127,7 @@ private: float _course_over_ground_y; // Course overground Y axis float _health; bool _centripetal; - bool _toggle; + uint8_t _toggle; }; #endif From e6b7c57d26e6905ef504d884ec27cd2744b90716 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 11:22:01 -0700 Subject: [PATCH 46/74] Circle Mode now working 250Hz loop implemented --- ArduCopter/ArduCopter.pde | 13 +++++-------- ArduCopter/motors.pde | 9 +++++---- ArduCopter/system.pde | 2 +- ArduCopter/test.pde | 2 +- 4 files changed, 12 insertions(+), 14 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 37331f0ce3..541a1ec586 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -527,7 +527,7 @@ void loop() { // We want this to execute fast // ---------------------------- - if (millis() - fast_loopTimer >= 5) { + if (millis() - fast_loopTimer >= 4) { //PORTK |= B00010000; delta_ms_fast_loop = millis() - fast_loopTimer; fast_loopTimer = millis(); @@ -543,6 +543,7 @@ void loop() fast_loop(); fast_loopTimeStamp = millis(); } + //PORTK &= B11101111; if (millis() - fiftyhz_loopTimer > 19) { delta_ms_fiftyhz = millis() - fiftyhz_loopTimer; @@ -577,7 +578,6 @@ void loop() } //PORTK &= B10111111; } - //PORTK &= B11101111; } // PORTK |= B01000000; // PORTK &= B10111111; @@ -1268,16 +1268,13 @@ static void update_navigation() // calculates desired Yaw update_auto_yaw(); { - circle_angle += dTnav; //1000 * (dTnav/1000); - - if (circle_angle >= 36000) - circle_angle -= 36000; + //circle_angle += dTnav; //1000 * (dTnav/1000); + circle_angle = wrap_360(target_bearing + 2000 + 18000); target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle)); target_WP.lat = next_WP.lat + g.loiter_radius * sin(radians(90 - circle_angle)); } - // calc the lat and long error to the target calc_location_error(&target_WP); @@ -1301,7 +1298,7 @@ static void read_AHRS(void) hil.update(); #endif - dcm.update_DCM(G_Dt);//, _tog); + dcm.update_DCM_fast(G_Dt);//, _tog); omega = dcm.get_gyro(); } diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 4e64316376..5a4d0e3e9a 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -34,8 +34,9 @@ static void arm_motors() // Tune down DCM // ------------------- #if HIL_MODE != HIL_MODE_ATTITUDE - dcm.kp_roll_pitch(0.030000); - dcm.ki_roll_pitch(0.000006); + dcm.kp_roll_pitch(0.030000); + dcm.ki_roll_pitch(0.00001278), // 50 hz I term + //dcm.ki_roll_pitch(0.000006); #endif // tune down compass @@ -95,8 +96,8 @@ static void arm_motors() // Tune down DCM // ------------------- #if HIL_MODE != HIL_MODE_ATTITUDE - dcm.kp_roll_pitch(0.12); // higher for quads - dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop + dcm.kp_roll_pitch(0.12); // higher for fast recovery + //dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop #endif // tune up compass diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 3b8ef34edd..70daa8e178 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "ArduCopter 2.0.41 Beta", main_menu_commands); +MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands); #endif // CLI_ENABLED diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 6dcd5614f4..895b41d55b 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -447,7 +447,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) accels.x, accels.y, accels.z, gyros.x, gyros.y, gyros.z); */ - ///* + /* 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, From ad4fc23eea4ceffed5b3bdfaaac71a4810cd98f2 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Mon, 12 Sep 2011 02:24:00 +0800 Subject: [PATCH 47/74] firmware build --- .../Firmware/AC2-Hexa-1280.size.txt | 7 +- .../Firmware/AC2-Hexa-2560.size.txt | 7 +- .../Firmware/AC2-QUADHIL-1280.build.log | 155 +---- .../Firmware/AC2-QUADHIL-1280.size.txt | 607 ------------------ .../Firmware/AC2-QUADHIL-2560.build.log | 155 +---- .../Firmware/AC2-QUADHIL-2560.size.txt | 607 ------------------ .../Firmware/AC2-Quad-1280.size.txt | 7 +- .../Firmware/AC2-Quad-2560.size.txt | 7 +- .../Firmware/AC2-Tri-1280.size.txt | 7 +- .../Firmware/AC2-Tri-2560.size.txt | 7 +- .../Firmware/AC2-Y6-1280.size.txt | 7 +- .../Firmware/AC2-Y6-2560.size.txt | 7 +- .../Firmware/firmware2.xml | 12 +- Tools/ArdupilotMegaPlanner/Firmware/git.log | 17 +- 14 files changed, 48 insertions(+), 1561 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt index ee9cf57691..9ff2b1f235 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -592,15 +591,15 @@ 00000172 t update_nav_wp() 000001a0 t init_home() 000001a8 t print_radio_values() +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 000001fc t setup_motors(unsigned char, Menu::arg const*) 00000200 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001fb8 T loop +00001fa2 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt index 84dec1b6a0..bc16bd7f7a 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -592,15 +591,15 @@ 00000172 t update_nav_wp() 000001a0 t init_home() 000001a8 t print_radio_values() +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 000001fc t setup_motors(unsigned char, Menu::arg const*) 00000200 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001fb6 T loop +00001fa0 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log index b1aee4e1a4..48387c886f 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log @@ -3,6 +3,9 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde: In function 'void read_AHRS()': +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1301: error: 'class AP_DCM_HIL' has no member named 'update_DCM_fast' +autogenerated: At global scope: autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -63,154 +66,4 @@ autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never de /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used /root/apm/ardupilot-mega/ArduCopter/test.pde:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:24: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined -%% libraries/APM_BMP085/APM_BMP085.o -%% libraries/APM_PI/APM_PI.o -%% libraries/APM_RC/APM_RC.o -%% libraries/AP_ADC/AP_ADC_ADS7844.o -%% libraries/AP_ADC/AP_ADC.o -%% libraries/AP_ADC/AP_ADC_HIL.o -%% libraries/AP_Common/AP_Common.o -%% libraries/AP_Common/AP_Loop.o -%% libraries/AP_Common/AP_MetaClass.o -%% libraries/AP_Common/AP_Var.o -%% libraries/AP_Common/AP_Var_menufuncs.o -%% libraries/AP_Common/c++.o -%% libraries/AP_Common/menu.o -%% libraries/AP_Compass/AP_Compass_HIL.o -%% libraries/AP_Compass/AP_Compass_HMC5843.o -%% libraries/AP_Compass/Compass.o -%% libraries/AP_DCM/AP_DCM.o -%% libraries/AP_DCM/AP_DCM_HIL.o -%% libraries/AP_GPS/AP_GPS_406.o -%% libraries/AP_GPS/AP_GPS_Auto.o -%% libraries/AP_GPS/AP_GPS_HIL.o -%% libraries/AP_GPS/AP_GPS_IMU.o -%% libraries/AP_GPS/AP_GPS_MTK16.o -%% libraries/AP_GPS/AP_GPS_MTK.o -%% libraries/AP_GPS/AP_GPS_NMEA.o -%% libraries/AP_GPS/AP_GPS_SIRF.o -%% libraries/AP_GPS/AP_GPS_UBLOX.o -%% libraries/AP_GPS/GPS.o -%% libraries/AP_IMU/AP_IMU_Oilpan.o -%% libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.o -In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:28: -/usr/local/share/arduino/libraries/SPI/../SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined -/usr/local/share/arduino/libraries/SPI/../SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'void AP_OpticalFlow_ADNS3080::set_led_always_on(bool)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:221: warning: suggest parentheses around arithmetic in operand of | -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'bool AP_OpticalFlow_ADNS3080::get_frame_rate_auto()': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:257: warning: suggest parentheses around comparison in operand of & -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:361: warning: suggest parentheses around arithmetic in operand of | -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:408: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'int AP_OpticalFlow_ADNS3080::print_pixel_data(Stream*)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:441: warning: suggest parentheses around comparison in operand of & -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:456: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:456: warning: control reaches end of non-void function -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:408: warning: control reaches end of non-void function -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'virtual bool AP_OpticalFlow_ADNS3080::init(bool)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:80: warning: control reaches end of non-void function -%% libraries/AP_OpticalFlow/AP_OpticalFlow.o -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual int AP_OpticalFlow::read()': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:38: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual byte AP_OpticalFlow::read_register(byte)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:44: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual void AP_OpticalFlow::get_position(float, float, float, float)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:88: warning: unused variable 'i' -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual byte AP_OpticalFlow::read_register(byte)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:44: warning: control reaches end of non-void function -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual int AP_OpticalFlow::read()': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:38: warning: control reaches end of non-void function -%% libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.o -%% libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.o -%% libraries/AP_RangeFinder/RangeFinder.o -%% libraries/DataFlash/DataFlash.o -In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35: -/usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined -/usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -%% libraries/FastSerial/BetterStream.o -%% libraries/FastSerial/FastSerial.o -%% libraries/FastSerial/vprintf.o -%% libraries/GCS_MAVLink/GCS_MAVLink.o -%% libraries/ModeFilter/ModeFilter.o -%% libraries/RC_Channel/RC_Channel.o -%% libraries/memcheck/memcheck.o -%% libraries/FastSerial/ftoa_engine.o -%% libraries/FastSerial/ultoa_invert.o -%% libraries/SPI/SPI.o -In file included from /usr/local/share/arduino/libraries/SPI/SPI.cpp:12: -/usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined -/usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -%% libraries/Wire/Wire.o -%% libraries/Wire/utility/twi.o -cc1: warning: command line option "-Wno-reorder" is valid for C++/ObjC++ but not for C -%% arduino/HardwareSerial.o -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/HardwareSerial.cpp:28: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -%% arduino/main.o -%% arduino/Print.o -%% arduino/Tone.o -/usr/local/share/arduino/hardware/arduino/cores/arduino/Tone.cpp:93: warning: only initialized variables can be placed into program memory area -%% arduino/WMath.o -%% arduino/WString.o -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:26: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:84: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:85: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:86: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:87: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:88: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:89: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:90: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:91: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:93: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:94: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:95: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:100: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:101: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:102: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:103: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:104: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:105: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:106: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:107: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:109: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:110: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:111: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:116: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:117: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:118: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:119: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:120: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:121: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:122: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:123: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:125: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:126: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:127: warning: initialization makes integer from pointer without a cast -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/WInterrupts.c:34: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_analog.c:27: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring.c:25: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_digital.c:27: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_pulse.c:25: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_shift.c:25: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -%% arduino/core.a -%% ArduCopter.elf -%% ArduCopter.eep -%% ArduCopter.hex +make: *** [/tmp/ArduCopter.build/ArduCopter.o] Error 1 diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt index 300eeeb23c..e69de29bb2 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt @@ -1,607 +0,0 @@ -00000001 b wp_control -00000001 b GPS_enabled -00000001 b home_is_set -00000001 b motor_armed -00000001 b motor_light -00000001 b control_mode -00000001 b simple_timer -00000001 d yaw_tracking -00000001 b land_complete -00000001 b throttle_mode -00000001 b command_may_ID -00000001 b wp_verify_byte -00000001 b xtrack_enabled -00000001 d altitude_sensor -00000001 b command_must_ID -00000001 b command_yaw_dir -00000001 b roll_pitch_mode -00000001 b counter_one_herz -00000001 b delta_ms_fiftyhz -00000001 b did_ground_start -00000001 b in_mavlink_delay -00000001 b invalid_throttle -00000001 b motor_auto_armed -00000001 b old_control_mode -00000001 b slow_loopCounter -00000001 b takeoff_complete -00000001 b command_may_index -00000001 b oldSwitchPosition -00000001 b command_must_index -00000001 b delta_ms_fast_loop -00000001 d ground_start_count -00000001 b medium_loopCounter -00000001 b command_yaw_relative -00000001 b delta_ms_medium_loop -00000001 b event_id -00000001 b led_mode -00000001 b yaw_mode -00000001 b GPS_light -00000001 b alt_timer -00000001 b loop_step -00000001 b trim_flag -00000001 b dancing_light()::step -00000001 b update_motor_leds()::blink -00000001 b radio_input_switch()::bouncer -00000001 b read_control_switch()::switch_debouncer -00000001 d GCS_MAVLINK::handleMessage(__mavlink_message*)::mav_nav -00000001 B mavdelay -00000002 T mavlink_acknowledge(mavlink_channel_t, unsigned char, unsigned char, unsigned char) -00000002 b climb_rate -00000002 b event_delay -00000002 b event_value -00000002 b event_repeat -00000002 b nav_throttle -00000002 b altitude_rate -00000002 b gps_fix_count -00000002 b velocity_land -00000002 b mainLoop_count -00000002 b command_yaw_time -00000002 b event_undo_value -00000002 b command_yaw_speed -00000002 b auto_level_counter -00000002 b superslow_loopCounter -00000002 r comma -00000002 b g_gps -00000002 b G_Dt_max -00000002 b airspeed -00000002 b sonar_alt -00000002 W AP_IMU_Shim::init_accel(void (*)(unsigned long)) -00000002 W AP_IMU_Shim::init(IMU::Start_style, void (*)(unsigned long)) -00000002 W AP_IMU_Shim::init_gyro(void (*)(unsigned long)) -00000002 T GCS_MAVLINK::acknowledge(unsigned char, unsigned char, unsigned char) -00000002 b arm_motors()::arming_counter -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r test_wp(unsigned char, Menu::arg const*)::__c -00000002 V APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)::__c -00000002 V APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)::__c -00000002 B x_actual_speed -00000002 B x_rate_error -00000002 B y_actual_speed -00000002 B y_rate_error -00000003 r select_logs(unsigned char, Menu::arg const*)::__c -00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c -00000003 r print_enabled(unsigned char)::__c -00000003 r setup_compass(unsigned char, Menu::arg const*)::__c -00000004 d cos_roll_x -00000004 b land_start -00000004 b long_error -00000004 b sin_roll_y -00000004 d cos_pitch_x -00000004 b event_timer -00000004 b loiter_time -00000004 b nav_bearing -00000004 d scaleLongUp -00000004 b sin_pitch_y -00000004 b wp_distance -00000004 b circle_angle -00000004 b current_amps -00000004 b gps_base_alt -00000004 b original_alt -00000004 b bearing_error -00000004 b current_total -00000004 b nav_loopTimer -00000004 d scaleLongDown -00000004 t test_failsafe(unsigned char, Menu::arg const*) -00000004 b altitude_error -00000004 b fast_loopTimer -00000004 b perf_mon_timer -00000004 b target_bearing -00000004 d battery_voltage -00000004 b command_yaw_end -00000004 b condition_start -00000004 b condition_value -00000004 b loiter_time_max -00000004 b target_altitude -00000004 d battery_voltage1 -00000004 d battery_voltage2 -00000004 d battery_voltage3 -00000004 d battery_voltage4 -00000004 b medium_loopTimer -00000004 b wp_totalDistance -00000004 b command_yaw_delta -00000004 b command_yaw_start -00000004 b fiftyhz_loopTimer -00000004 b crosstrack_bearing -00000004 b fast_loopTimeStamp -00000004 b throttle_integrator -00000004 b saved_target_bearing -00000004 r __menu_name__log_menu -00000004 b command_yaw_start_time -00000004 b initial_simple_bearing -00000004 d G_Dt -00000004 b dTnav -00000004 b nav_lat -00000004 b nav_lon -00000004 b nav_yaw -00000004 b old_alt -00000004 b auto_yaw -00000004 b nav_roll -00000004 d cos_yaw_x -00000004 b lat_error -00000004 b nav_pitch -00000004 b sin_yaw_y -00000004 b yaw_error -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r setup_sonar(unsigned char, Menu::arg const*)::__c -00000004 b mavlink_delay(unsigned long)::last_1hz -00000004 b mavlink_delay(unsigned long)::last_3hz -00000004 b mavlink_delay(unsigned long)::last_10hz -00000004 b mavlink_delay(unsigned long)::last_50hz -00000004 r print_enabled(unsigned char)::__c -00000004 r setup_compass(unsigned char, Menu::arg const*)::__c -00000004 r print_log_menu()::__c -00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c -00000004 V Parameters::Parameters()::__c -00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000004 B tuning_value -00000005 r __menu_name__test_menu -00000005 r report_imu()::__c -00000005 r select_logs(unsigned char, Menu::arg const*)::__c -00000005 r select_logs(unsigned char, Menu::arg const*)::__c -00000005 r select_logs(unsigned char, Menu::arg const*)::__c -00000005 r Log_Read_Raw()::__c -00000005 r Log_Read_Mode()::__c -00000005 r report_tuning()::__c -00000005 r print_log_menu()::__c -00000005 r print_log_menu()::__c -00000005 r print_log_menu()::__c -00000005 r print_log_menu()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000005 V APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)::__c -00000006 r __menu_name__setup_menu -00000006 r report_gps()::__c -00000006 r test_eedump(unsigned char, Menu::arg const*)::__c -00000006 r test_eedump(unsigned char, Menu::arg const*)::__c -00000006 r zero_eeprom()::__c -00000006 r Log_Read_Mode()::__c -00000006 r print_log_menu()::__c -00000006 r print_log_menu()::__c -00000006 V Parameters::Parameters()::__c -00000007 b setup_menu -00000007 b planner_menu -00000007 b log_menu -00000007 b main_menu -00000007 b test_menu -00000007 r select_logs(unsigned char, Menu::arg const*)::__c -00000007 r select_logs(unsigned char, Menu::arg const*)::__c -00000007 r report_frame()::__c -00000007 r report_radio()::__c -00000007 r report_sonar()::__c -00000007 r print_enabled(unsigned char)::__c -00000007 r test_wp(unsigned char, Menu::arg const*)::__c -00000007 V Parameters::Parameters()::__c -00000007 V Parameters::Parameters()::__c -00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000008 t setup_erase(unsigned char, Menu::arg const*) -00000008 r __menu_name__planner_menu -00000008 W AP_IMU_Shim::update() -00000008 r select_logs(unsigned char, Menu::arg const*)::__c -00000008 r report_frame()::__c -00000008 r report_frame()::__c -00000008 r report_frame()::__c -00000008 r report_tuning()::__c -00000008 r print_log_menu()::__c -00000008 r report_batt_monitor()::__c -00000008 r report_batt_monitor()::__c -00000008 r test_wp(unsigned char, Menu::arg const*)::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r print_switch(unsigned char, unsigned char)::__c -00000009 r print_log_menu()::__c -00000009 r print_log_menu()::__c -00000009 r report_compass()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r start_new_log()::__c -0000000a r print_log_menu()::__c -0000000a r Log_Read_Startup()::__c -0000000a r test_wp(unsigned char, Menu::arg const*)::__c -0000000a r test_mag(unsigned char, Menu::arg const*)::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a T setup -0000000b r test_relay(unsigned char, Menu::arg const*)::__c -0000000b r report_batt_monitor()::__c -0000000b V Parameters::Parameters()::__c -0000000c t setup_accel(unsigned char, Menu::arg const*) -0000000c t process_logs(unsigned char, Menu::arg const*) -0000000c b omega -0000000c t test_mode(unsigned char, Menu::arg const*) -0000000c T GCS_MAVLINK::send_text(unsigned char, char const*) -0000000c V vtable for AP_IMU_Shim -0000000c V vtable for IMU -0000000c r report_frame()::__c -0000000c V Parameters::Parameters()::__c -0000000c V Parameters::Parameters()::__c -0000000c V Parameters::Parameters()::__c -0000000d r verify_RTL()::__c -0000000d r select_logs(unsigned char, Menu::arg const*)::__c -0000000d r test_tuning(unsigned char, Menu::arg const*)::__c -0000000d r test_battery(unsigned char, Menu::arg const*)::__c -0000000d r startup_ground()::__c -0000000d r test_wp(unsigned char, Menu::arg const*)::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d B sonar_mode_filter -0000000e t global destructors keyed to Serial -0000000e t global constructors keyed to Serial -0000000e V vtable for AP_Float16 -0000000e V vtable for AP_VarS > -0000000e V vtable for AP_VarS > -0000000e V vtable for AP_VarT -0000000e V vtable for AP_VarT -0000000e V vtable for AP_VarT -0000000e r arm_motors()::__c -0000000e r erase_logs(unsigned char, Menu::arg const*)::__c -0000000e r setup_mode(unsigned char, Menu::arg const*)::__c -0000000e r test_sonar(unsigned char, Menu::arg const*)::__c -0000000e r select_logs(unsigned char, Menu::arg const*)::__c -0000000e r print_log_menu()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r report_batt_monitor()::__c -0000000e r report_flight_modes()::__c -0000000e r dump_log(unsigned char, Menu::arg const*)::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000f b current_loc -0000000f b next_command -0000000f b home -0000000f b next_WP -0000000f b prev_WP -0000000f b guided_WP -0000000f b target_WP -0000000f r print_log_menu()::__c -0000000f r print_log_menu()::__c -0000000f r report_version()::__c -0000000f r report_batt_monitor()::__c -0000000f r test_imu(unsigned char, Menu::arg const*)::__c -00000010 r planner_menu_commands -00000010 b motor_out -00000010 W AP_VarT::cast_to_float() const -00000010 r test_sonar(unsigned char, Menu::arg const*)::__c -00000010 r report_compass()::__c -00000010 t mavlink_get_channel_status -00000011 r arm_motors()::__c -00000011 r erase_logs(unsigned char, Menu::arg const*)::__c -00000011 r zero_eeprom()::__c -00000011 r update_commands()::__c -00000011 r Log_Read_Attitude()::__c -00000012 B Serial -00000012 B Serial1 -00000012 B Serial3 -00000012 r flight_mode_strings -00000012 W AP_Float16::~AP_Float16() -00000012 W AP_VarS >::~AP_VarS() -00000012 W AP_VarS >::~AP_VarS() -00000012 W AP_VarT::~AP_VarT() -00000012 W AP_VarT::~AP_VarT() -00000012 W AP_VarT::~AP_VarT() -00000012 W AP_VarT::serialize(void*, unsigned int) const -00000012 r print_done()::__c -00000012 r select_logs(unsigned char, Menu::arg const*)::__c -00000012 r setup_frame(unsigned char, Menu::arg const*)::__c -00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c -00000012 B adc -00000013 r setup_compass(unsigned char, Menu::arg const*)::__c -00000013 r change_command(unsigned char)::__c -00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c -00000014 t startup_ground() -00000014 W AP_VarT::unserialize(void*, unsigned int) -00000014 W AP_VarT::cast_to_float() const -00000014 W AP_VarT::cast_to_float() const -00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c -00000014 r test_tri(unsigned char, Menu::arg const*)::__c -00000015 r map_baudrate(signed char, unsigned long)::__c -00000015 r init_ardupilot()::__c -00000015 r Log_Read_Motors()::__c -00000015 r print_hit_enter()::__c -00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c -00000016 r init_ardupilot()::__c -00000016 r GCS_MAVLINK::update()::__c -00000016 B sonar -00000017 r __menu_name__main_menu -00000018 t setup_tune(unsigned char, Menu::arg const*) -00000018 W AP_VarT::serialize(void*, unsigned int) const -00000018 b mavlink_get_channel_status::m_mavlink_status -00000019 r GCS_MAVLINK::update()::__c -0000001a r print_log_menu()::__c -0000001c W AP_VarS >::unserialize(void*, unsigned int) -0000001c W AP_VarS >::unserialize(void*, unsigned int) -0000001c W AP_VarT::unserialize(void*, unsigned int) -0000001c W AP_VarS >::serialize(void*, unsigned int) const -0000001c W AP_VarS >::serialize(void*, unsigned int) const -0000001e r Log_Read_Optflow()::__c -0000001e r Log_Read_Nav_Tuning()::__c -0000001f r test_mag(unsigned char, Menu::arg const*)::__c -00000020 r test_current(unsigned char, Menu::arg const*)::__c -00000020 t byte_swap_4 -00000021 r print_log_menu()::__c -00000021 r report_compass()::__c -00000021 r Log_Read_Current()::__c -00000022 t clear_leds() -00000022 t print_blanks(int) -00000022 t reset_hold_I() -00000022 W AP_Float16::~AP_Float16() -00000022 W AP_VarS >::~AP_VarS() -00000022 W AP_VarS >::~AP_VarS() -00000022 W AP_VarT::~AP_VarT() -00000022 W AP_VarT::~AP_VarT() -00000022 W AP_VarT::~AP_VarT() -00000023 r setup_mode(unsigned char, Menu::arg const*)::__c -00000023 r print_gyro_offsets()::__c -00000024 r init_ardupilot()::__c -00000024 r print_accel_offsets()::__c -00000025 r setup_factory(unsigned char, Menu::arg const*)::__c -00000026 t print_done() -00000026 b mavlink_queue -00000026 t print_hit_enter() -00000026 t Log_Read_Startup() -00000027 r test_xbee(unsigned char, Menu::arg const*)::__c -00000028 t test_battery(unsigned char, Menu::arg const*) -00000028 t main_menu_help(unsigned char, Menu::arg const*) -00000028 t help_log(unsigned char, Menu::arg const*) -00000028 W AP_VarT::unserialize(void*, unsigned int) -00000028 W AP_VarT::serialize(void*, unsigned int) const -00000028 r Log_Read_Cmd()::__c -00000028 B imu -00000029 r setup_mode(unsigned char, Menu::arg const*)::__c -00000029 r test_gps(unsigned char, Menu::arg const*)::__c -0000002a t setup_declination(unsigned char, Menu::arg const*) -0000002a r Log_Read_Control_Tuning()::__c -0000002b r planner_mode(unsigned char, Menu::arg const*)::__c -0000002e t print_divider() -0000002e t send_rate(unsigned int, unsigned int) -0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char) -0000002e r Log_Read_Performance()::__c -0000002f r test_radio(unsigned char, Menu::arg const*)::__c -00000030 t planner_mode(unsigned char, Menu::arg const*) -00000032 T GCS_MAVLINK::init(BetterStream*) -00000032 W APM_PI::~APM_PI() -00000034 t _MAV_RETURN_float -00000034 W AP_Float16::serialize(void*, unsigned int) const -00000034 t _mav_put_int8_t_array -00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c -00000036 t report_radio() -00000036 r Log_Read_GPS()::__c -00000037 r print_wp(Location*, unsigned char)::__c -00000038 t init_throttle_cruise() -00000038 r setup_radio(unsigned char, Menu::arg const*)::__c -00000038 r setup_factory(unsigned char, Menu::arg const*)::__c -0000003a t report_gps() -0000003a t report_imu() -0000003a B g_gps_driver -0000003c W RC_Channel::~RC_Channel() -0000003e t verify_RTL() -0000003e T GCS_MAVLINK::send_text(unsigned char, prog_char_t const*) -0000003e W AP_VarT::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char) -00000040 W AP_Float16::unserialize(void*, unsigned int) -00000040 t byte_swap_8 -00000040 t crc_accumulate -00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c -00000044 t setup_show(unsigned char, Menu::arg const*) -00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) -0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) -0000004c t update_auto_yaw() -0000004e T mavlink_send_text(mavlink_channel_t, unsigned char, char const*) -00000050 r log_menu_commands -00000050 r main_menu_commands -00000050 T GCS_MAVLINK::_find_parameter(unsigned int) -00000052 t change_command(unsigned char) -00000052 t report_version() -00000054 t print_enabled(unsigned char) -00000054 t update_motor_leds() -00000054 t report_flight_modes() -00000055 r main_menu_help(unsigned char, Menu::arg const*)::__c -00000055 r setup_flightmodes(unsigned char, Menu::arg const*)::__c -00000056 t readSwitch() -00000056 t dancing_light() -00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000057 B dcm -0000005a t read_control_switch() -0000005c t get_num_logs() -0000005c t setup_esc(unsigned char, Menu::arg const*) -0000005e t update_GPS_light() -0000005e t radio_input_switch() -0000005e T GCS_MAVLINK::_count_parameters() -00000060 t _mavlink_send_uart -00000062 t print_switch(unsigned char, unsigned char) -00000064 t print_gyro_offsets() -00000064 t print_accel_offsets() -00000064 t test_xbee(unsigned char, Menu::arg const*) -00000064 t mavlink_msg_param_value_send -00000068 t zero_eeprom() -00000068 t find_last_log_page(int) -0000006a W GCS_MAVLINK::~GCS_MAVLINK() -0000006c t setup_sonar(unsigned char, Menu::arg const*) -0000006e T output_min() -00000078 t setup_batt_monitor(unsigned char, Menu::arg const*) -0000007a t setup_factory(unsigned char, Menu::arg const*) -0000007e t test_rawgps(unsigned char, Menu::arg const*) -00000080 T __vector_25 -00000080 T __vector_36 -00000080 T __vector_54 -00000082 t do_RTL() -00000086 t Log_Read_Attitude() -00000088 t Log_Read_Raw() -0000008c t setup_frame(unsigned char, Menu::arg const*) -00000090 t init_compass() -00000090 t dump_log(unsigned char, Menu::arg const*) -00000092 t test_tuning(unsigned char, Menu::arg const*) -00000092 t report_tuning() -00000095 r init_ardupilot()::__c -00000096 t map_baudrate(signed char, unsigned long) -00000096 t print_wp(Location*, unsigned char) -0000009a t Log_Read_Motors() -0000009d B gcs -0000009d B hil -0000009e t setup_mode(unsigned char, Menu::arg const*) -0000009e t Log_Write_Cmd(unsigned char, Location*) -000000a0 t Log_Read_Mode() -000000a4 T __vector_26 -000000a4 T __vector_37 -000000a4 T __vector_55 -000000aa t test_sonar(unsigned char, Menu::arg const*) -000000ab B compass -000000ae t report_frame() -000000b2 t erase_logs(unsigned char, Menu::arg const*) -000000b4 t test_relay(unsigned char, Menu::arg const*) -000000b4 t planner_gcs(unsigned char, Menu::arg const*) -000000b6 t get_log_boundaries(unsigned char, int&, int&) -000000be t Log_Read_Nav_Tuning() -000000c2 t setup_compass(unsigned char, Menu::arg const*) -000000c4 t get_distance(Location*, Location*) -000000c4 t update_events() -000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c -000000c6 t test_eedump(unsigned char, Menu::arg const*) -000000c6 t Log_Read(int, int) -000000c6 t test_tri(unsigned char, Menu::arg const*) -000000cc t read_radio() -000000d0 t get_bearing(Location*, Location*) -000000d8 t test_radio(unsigned char, Menu::arg const*) -000000de t Log_Read_Performance() -000000de t Log_Read_Control_Tuning() -000000e0 r setup_menu_commands -000000e0 b mavlink_parse_char::m_mavlink_message -000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) -000000e4 t Log_Read_Optflow() -000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&) -000000e6 t setup_flightmodes(unsigned char, Menu::arg const*) -000000e8 t Log_Read_Current() -000000ee t report_batt_monitor() -000000f4 t _mav_finalize_message_chan_send -000000f6 t Log_Read_Cmd() -000000fa t calc_nav_pitch_roll() -00000100 r test_menu_commands -0000010a t test_gps(unsigned char, Menu::arg const*) -0000010c W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) -00000112 t test_current(unsigned char, Menu::arg const*) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000118 t set_command_with_index(Location, int) -00000118 T GCS_MAVLINK::_queued_send() -00000126 t arm_motors() -00000128 t get_command_with_index(int) -00000130 t report_compass() -00000134 T GCS_MAVLINK::send_message(unsigned char, unsigned long) -0000014e T GCS_MAVLINK::update() -00000150 t update_trig() -00000152 t set_next_WP(Location*) -00000156 t Log_Read_GPS() -00000166 t select_logs(unsigned char, Menu::arg const*) -0000016c t update_commands() -00000170 t test_mag(unsigned char, Menu::arg const*) -00000172 t update_nav_wp() -000001a0 t init_home() -000001a8 t print_radio_values() -000001b6 t setup_motors(unsigned char, Menu::arg const*) -000001ca t mavlink_delay(unsigned long) -000001ce t start_new_log() -000001ea T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) -00000200 t set_mode(unsigned char) -00000220 t test_wp(unsigned char, Menu::arg const*) -00000228 t setup_radio(unsigned char, Menu::arg const*) -0000029e t test_imu(unsigned char, Menu::arg const*) -000002ea t tuning() -00000330 t calc_nav_rate(int, int, int, int) -00000358 T update_throttle_mode() -00000384 t print_log_menu() -000003be t read_battery() -000003dc T update_yaw_mode() -000004b2 t mavlink_parse_char -00000578 t __static_initialization_and_destruction_0(int, int) -000005ea T update_roll_pitch_mode() -00000616 t init_ardupilot() -000007dc b g -0000083c W Parameters::Parameters() -000008e4 t process_next_command() -00001196 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001494 T GCS_MAVLINK::handleMessage(__mavlink_message*) -00001902 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log index b1aee4e1a4..48387c886f 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log @@ -3,6 +3,9 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:53: /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde: In function 'void read_AHRS()': +/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:1301: error: 'class AP_DCM_HIL' has no member named 'update_DCM_fast' +autogenerated: At global scope: autogenerated:34: warning: 'int alt_hold_velocity()' declared 'static' but never defined autogenerated:86: warning: 'void acknowledge(byte, byte, byte)' declared 'static' but never defined autogenerated:87: warning: 'void send_message(byte)' declared 'static' but never defined @@ -63,154 +66,4 @@ autogenerated:319: warning: 'void fake_out_gps()' declared 'static' but never de /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:469: warning: 'new_location' defined but not used /root/apm/ardupilot-mega/ArduCopter/test.pde:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:24: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined -%% libraries/APM_BMP085/APM_BMP085.o -%% libraries/APM_PI/APM_PI.o -%% libraries/APM_RC/APM_RC.o -%% libraries/AP_ADC/AP_ADC_ADS7844.o -%% libraries/AP_ADC/AP_ADC.o -%% libraries/AP_ADC/AP_ADC_HIL.o -%% libraries/AP_Common/AP_Common.o -%% libraries/AP_Common/AP_Loop.o -%% libraries/AP_Common/AP_MetaClass.o -%% libraries/AP_Common/AP_Var.o -%% libraries/AP_Common/AP_Var_menufuncs.o -%% libraries/AP_Common/c++.o -%% libraries/AP_Common/menu.o -%% libraries/AP_Compass/AP_Compass_HIL.o -%% libraries/AP_Compass/AP_Compass_HMC5843.o -%% libraries/AP_Compass/Compass.o -%% libraries/AP_DCM/AP_DCM.o -%% libraries/AP_DCM/AP_DCM_HIL.o -%% libraries/AP_GPS/AP_GPS_406.o -%% libraries/AP_GPS/AP_GPS_Auto.o -%% libraries/AP_GPS/AP_GPS_HIL.o -%% libraries/AP_GPS/AP_GPS_IMU.o -%% libraries/AP_GPS/AP_GPS_MTK16.o -%% libraries/AP_GPS/AP_GPS_MTK.o -%% libraries/AP_GPS/AP_GPS_NMEA.o -%% libraries/AP_GPS/AP_GPS_SIRF.o -%% libraries/AP_GPS/AP_GPS_UBLOX.o -%% libraries/AP_GPS/GPS.o -%% libraries/AP_IMU/AP_IMU_Oilpan.o -%% libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.o -In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:28: -/usr/local/share/arduino/libraries/SPI/../SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined -/usr/local/share/arduino/libraries/SPI/../SPI/SPI.h:20:1: warning: this is the location of the previous definition -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'void AP_OpticalFlow_ADNS3080::set_led_always_on(bool)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:221: warning: suggest parentheses around arithmetic in operand of | -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'bool AP_OpticalFlow_ADNS3080::get_frame_rate_auto()': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:257: warning: suggest parentheses around comparison in operand of & -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:361: warning: suggest parentheses around arithmetic in operand of | -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:408: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'int AP_OpticalFlow_ADNS3080::print_pixel_data(Stream*)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:441: warning: suggest parentheses around comparison in operand of & -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:456: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:456: warning: control reaches end of non-void function -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:408: warning: control reaches end of non-void function -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp: In member function 'virtual bool AP_OpticalFlow_ADNS3080::init(bool)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp:80: warning: control reaches end of non-void function -%% libraries/AP_OpticalFlow/AP_OpticalFlow.o -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual int AP_OpticalFlow::read()': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:38: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual byte AP_OpticalFlow::read_register(byte)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:44: warning: no return statement in function returning non-void -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual void AP_OpticalFlow::get_position(float, float, float, float)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:88: warning: unused variable 'i' -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual byte AP_OpticalFlow::read_register(byte)': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:44: warning: control reaches end of non-void function -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp: In member function 'virtual int AP_OpticalFlow::read()': -/root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp:38: warning: control reaches end of non-void function -%% libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.o -%% libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.o -%% libraries/AP_RangeFinder/RangeFinder.o -%% libraries/DataFlash/DataFlash.o -In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35: -/usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined -/usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -%% libraries/FastSerial/BetterStream.o -%% libraries/FastSerial/FastSerial.o -%% libraries/FastSerial/vprintf.o -%% libraries/GCS_MAVLink/GCS_MAVLink.o -%% libraries/ModeFilter/ModeFilter.o -%% libraries/RC_Channel/RC_Channel.o -%% libraries/memcheck/memcheck.o -%% libraries/FastSerial/ftoa_engine.o -%% libraries/FastSerial/ultoa_invert.o -%% libraries/SPI/SPI.o -In file included from /usr/local/share/arduino/libraries/SPI/SPI.cpp:12: -/usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined -/usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition -%% libraries/Wire/Wire.o -%% libraries/Wire/utility/twi.o -cc1: warning: command line option "-Wno-reorder" is valid for C++/ObjC++ but not for C -%% arduino/HardwareSerial.o -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/HardwareSerial.cpp:28: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -%% arduino/main.o -%% arduino/Print.o -%% arduino/Tone.o -/usr/local/share/arduino/hardware/arduino/cores/arduino/Tone.cpp:93: warning: only initialized variables can be placed into program memory area -%% arduino/WMath.o -%% arduino/WString.o -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:26: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:84: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:85: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:86: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:87: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:88: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:89: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:90: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:91: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:93: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:94: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:95: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:100: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:101: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:102: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:103: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:104: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:105: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:106: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:107: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:109: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:110: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:111: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:116: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:117: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:118: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:119: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:120: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:121: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:122: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:123: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:125: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:126: warning: initialization makes integer from pointer without a cast -/usr/local/share/arduino/hardware/arduino/cores/arduino/pins_arduino.c:127: warning: initialization makes integer from pointer without a cast -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/WInterrupts.c:34: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_analog.c:27: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring.c:25: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_digital.c:27: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_pulse.c:25: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -In file included from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_private.h:30, - from /usr/local/share/arduino/hardware/arduino/cores/arduino/wiring_shift.c:25: -/usr/lib/gcc/avr/4.3.5/../../../avr/include/avr/delay.h:36:2: warning: #warning "This file has been moved to ." -%% arduino/core.a -%% ArduCopter.elf -%% ArduCopter.eep -%% ArduCopter.hex +make: *** [/tmp/ArduCopter.build/ArduCopter.o] Error 1 diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt index 9781e6a0e6..e69de29bb2 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt @@ -1,607 +0,0 @@ -00000001 b wp_control -00000001 b GPS_enabled -00000001 b home_is_set -00000001 b motor_armed -00000001 b motor_light -00000001 b control_mode -00000001 b simple_timer -00000001 d yaw_tracking -00000001 b land_complete -00000001 b throttle_mode -00000001 b command_may_ID -00000001 b wp_verify_byte -00000001 b xtrack_enabled -00000001 d altitude_sensor -00000001 b command_must_ID -00000001 b command_yaw_dir -00000001 b roll_pitch_mode -00000001 b counter_one_herz -00000001 b delta_ms_fiftyhz -00000001 b did_ground_start -00000001 b in_mavlink_delay -00000001 b invalid_throttle -00000001 b motor_auto_armed -00000001 b old_control_mode -00000001 b slow_loopCounter -00000001 b takeoff_complete -00000001 b command_may_index -00000001 b oldSwitchPosition -00000001 b command_must_index -00000001 b delta_ms_fast_loop -00000001 d ground_start_count -00000001 b medium_loopCounter -00000001 b command_yaw_relative -00000001 b delta_ms_medium_loop -00000001 b event_id -00000001 b led_mode -00000001 b yaw_mode -00000001 b GPS_light -00000001 b alt_timer -00000001 b loop_step -00000001 b trim_flag -00000001 b dancing_light()::step -00000001 b update_motor_leds()::blink -00000001 b radio_input_switch()::bouncer -00000001 b read_control_switch()::switch_debouncer -00000001 d GCS_MAVLINK::handleMessage(__mavlink_message*)::mav_nav -00000001 B mavdelay -00000002 T mavlink_acknowledge(mavlink_channel_t, unsigned char, unsigned char, unsigned char) -00000002 b climb_rate -00000002 b event_delay -00000002 b event_value -00000002 b event_repeat -00000002 b nav_throttle -00000002 b altitude_rate -00000002 b gps_fix_count -00000002 b velocity_land -00000002 b mainLoop_count -00000002 b command_yaw_time -00000002 b event_undo_value -00000002 b command_yaw_speed -00000002 b auto_level_counter -00000002 b superslow_loopCounter -00000002 r comma -00000002 b g_gps -00000002 b G_Dt_max -00000002 b airspeed -00000002 b sonar_alt -00000002 W AP_IMU_Shim::init_accel(void (*)(unsigned long)) -00000002 W AP_IMU_Shim::init(IMU::Start_style, void (*)(unsigned long)) -00000002 W AP_IMU_Shim::init_gyro(void (*)(unsigned long)) -00000002 T GCS_MAVLINK::acknowledge(unsigned char, unsigned char, unsigned char) -00000002 b arm_motors()::arming_counter -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r setup_frame(unsigned char, Menu::arg const*)::__c -00000002 r test_wp(unsigned char, Menu::arg const*)::__c -00000002 V APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)::__c -00000002 V APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)::__c -00000002 B x_actual_speed -00000002 B x_rate_error -00000002 B y_actual_speed -00000002 B y_rate_error -00000003 r select_logs(unsigned char, Menu::arg const*)::__c -00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c -00000003 r print_enabled(unsigned char)::__c -00000003 r setup_compass(unsigned char, Menu::arg const*)::__c -00000004 d cos_roll_x -00000004 b land_start -00000004 b long_error -00000004 b sin_roll_y -00000004 d cos_pitch_x -00000004 b event_timer -00000004 b loiter_time -00000004 b nav_bearing -00000004 d scaleLongUp -00000004 b sin_pitch_y -00000004 b wp_distance -00000004 b circle_angle -00000004 b current_amps -00000004 b gps_base_alt -00000004 b original_alt -00000004 b bearing_error -00000004 b current_total -00000004 b nav_loopTimer -00000004 d scaleLongDown -00000004 t test_failsafe(unsigned char, Menu::arg const*) -00000004 b altitude_error -00000004 b fast_loopTimer -00000004 b perf_mon_timer -00000004 b target_bearing -00000004 d battery_voltage -00000004 b command_yaw_end -00000004 b condition_start -00000004 b condition_value -00000004 b loiter_time_max -00000004 b target_altitude -00000004 d battery_voltage1 -00000004 d battery_voltage2 -00000004 d battery_voltage3 -00000004 d battery_voltage4 -00000004 b medium_loopTimer -00000004 b wp_totalDistance -00000004 b command_yaw_delta -00000004 b command_yaw_start -00000004 b fiftyhz_loopTimer -00000004 b crosstrack_bearing -00000004 b fast_loopTimeStamp -00000004 b throttle_integrator -00000004 b saved_target_bearing -00000004 r __menu_name__log_menu -00000004 b command_yaw_start_time -00000004 b initial_simple_bearing -00000004 d G_Dt -00000004 b dTnav -00000004 b nav_lat -00000004 b nav_lon -00000004 b nav_yaw -00000004 b old_alt -00000004 b auto_yaw -00000004 b nav_roll -00000004 d cos_yaw_x -00000004 b lat_error -00000004 b nav_pitch -00000004 b sin_yaw_y -00000004 b yaw_error -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r select_logs(unsigned char, Menu::arg const*)::__c -00000004 r setup_sonar(unsigned char, Menu::arg const*)::__c -00000004 b mavlink_delay(unsigned long)::last_1hz -00000004 b mavlink_delay(unsigned long)::last_3hz -00000004 b mavlink_delay(unsigned long)::last_10hz -00000004 b mavlink_delay(unsigned long)::last_50hz -00000004 r print_enabled(unsigned char)::__c -00000004 r setup_compass(unsigned char, Menu::arg const*)::__c -00000004 r print_log_menu()::__c -00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c -00000004 V Parameters::Parameters()::__c -00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000004 B tuning_value -00000005 r __menu_name__test_menu -00000005 r report_imu()::__c -00000005 r select_logs(unsigned char, Menu::arg const*)::__c -00000005 r select_logs(unsigned char, Menu::arg const*)::__c -00000005 r select_logs(unsigned char, Menu::arg const*)::__c -00000005 r Log_Read_Raw()::__c -00000005 r Log_Read_Mode()::__c -00000005 r report_tuning()::__c -00000005 r print_log_menu()::__c -00000005 r print_log_menu()::__c -00000005 r print_log_menu()::__c -00000005 r print_log_menu()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V Parameters::Parameters()::__c -00000005 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c -00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000005 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000005 V APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)::__c -00000006 r __menu_name__setup_menu -00000006 r report_gps()::__c -00000006 r test_eedump(unsigned char, Menu::arg const*)::__c -00000006 r test_eedump(unsigned char, Menu::arg const*)::__c -00000006 r zero_eeprom()::__c -00000006 r Log_Read_Mode()::__c -00000006 r print_log_menu()::__c -00000006 r print_log_menu()::__c -00000006 V Parameters::Parameters()::__c -00000007 b setup_menu -00000007 b planner_menu -00000007 b log_menu -00000007 b main_menu -00000007 b test_menu -00000007 r select_logs(unsigned char, Menu::arg const*)::__c -00000007 r select_logs(unsigned char, Menu::arg const*)::__c -00000007 r report_frame()::__c -00000007 r report_radio()::__c -00000007 r report_sonar()::__c -00000007 r print_enabled(unsigned char)::__c -00000007 r test_wp(unsigned char, Menu::arg const*)::__c -00000007 V Parameters::Parameters()::__c -00000007 V Parameters::Parameters()::__c -00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000008 t setup_erase(unsigned char, Menu::arg const*) -00000008 r __menu_name__planner_menu -00000008 W AP_IMU_Shim::update() -00000008 r select_logs(unsigned char, Menu::arg const*)::__c -00000008 r report_frame()::__c -00000008 r report_frame()::__c -00000008 r report_frame()::__c -00000008 r report_tuning()::__c -00000008 r print_log_menu()::__c -00000008 r report_batt_monitor()::__c -00000008 r report_batt_monitor()::__c -00000008 r test_wp(unsigned char, Menu::arg const*)::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 V Parameters::Parameters()::__c -00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r print_switch(unsigned char, unsigned char)::__c -00000009 r print_log_menu()::__c -00000009 r print_log_menu()::__c -00000009 r report_compass()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 V Parameters::Parameters()::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c -0000000a r test_relay(unsigned char, Menu::arg const*)::__c -0000000a r start_new_log()::__c -0000000a r print_log_menu()::__c -0000000a r Log_Read_Startup()::__c -0000000a r test_wp(unsigned char, Menu::arg const*)::__c -0000000a r test_mag(unsigned char, Menu::arg const*)::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a V Parameters::Parameters()::__c -0000000a T setup -0000000b r test_relay(unsigned char, Menu::arg const*)::__c -0000000b r report_batt_monitor()::__c -0000000b V Parameters::Parameters()::__c -0000000c t setup_accel(unsigned char, Menu::arg const*) -0000000c t process_logs(unsigned char, Menu::arg const*) -0000000c b omega -0000000c t test_mode(unsigned char, Menu::arg const*) -0000000c T GCS_MAVLINK::send_text(unsigned char, char const*) -0000000c V vtable for AP_IMU_Shim -0000000c V vtable for IMU -0000000c r report_frame()::__c -0000000c V Parameters::Parameters()::__c -0000000c V Parameters::Parameters()::__c -0000000c V Parameters::Parameters()::__c -0000000d r verify_RTL()::__c -0000000d r select_logs(unsigned char, Menu::arg const*)::__c -0000000d r test_tuning(unsigned char, Menu::arg const*)::__c -0000000d r test_battery(unsigned char, Menu::arg const*)::__c -0000000d r startup_ground()::__c -0000000d r test_wp(unsigned char, Menu::arg const*)::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d V Parameters::Parameters()::__c -0000000d B sonar_mode_filter -0000000e t global destructors keyed to Serial -0000000e t global constructors keyed to Serial -0000000e V vtable for AP_Float16 -0000000e V vtable for AP_VarS > -0000000e V vtable for AP_VarS > -0000000e V vtable for AP_VarT -0000000e V vtable for AP_VarT -0000000e V vtable for AP_VarT -0000000e r arm_motors()::__c -0000000e r erase_logs(unsigned char, Menu::arg const*)::__c -0000000e r setup_mode(unsigned char, Menu::arg const*)::__c -0000000e r test_sonar(unsigned char, Menu::arg const*)::__c -0000000e r select_logs(unsigned char, Menu::arg const*)::__c -0000000e r print_log_menu()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r print_radio_values()::__c -0000000e r report_batt_monitor()::__c -0000000e r report_flight_modes()::__c -0000000e r dump_log(unsigned char, Menu::arg const*)::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000e V Parameters::Parameters()::__c -0000000f b current_loc -0000000f b next_command -0000000f b home -0000000f b next_WP -0000000f b prev_WP -0000000f b guided_WP -0000000f b target_WP -0000000f r print_log_menu()::__c -0000000f r print_log_menu()::__c -0000000f r report_version()::__c -0000000f r report_batt_monitor()::__c -0000000f r test_imu(unsigned char, Menu::arg const*)::__c -00000010 r planner_menu_commands -00000010 b motor_out -00000010 W AP_VarT::cast_to_float() const -00000010 r test_sonar(unsigned char, Menu::arg const*)::__c -00000010 r report_compass()::__c -00000010 t mavlink_get_channel_status -00000011 r arm_motors()::__c -00000011 r erase_logs(unsigned char, Menu::arg const*)::__c -00000011 r zero_eeprom()::__c -00000011 r update_commands()::__c -00000011 r Log_Read_Attitude()::__c -00000012 B Serial -00000012 B Serial1 -00000012 B Serial3 -00000012 r flight_mode_strings -00000012 W AP_Float16::~AP_Float16() -00000012 W AP_VarS >::~AP_VarS() -00000012 W AP_VarS >::~AP_VarS() -00000012 W AP_VarT::~AP_VarT() -00000012 W AP_VarT::~AP_VarT() -00000012 W AP_VarT::~AP_VarT() -00000012 W AP_VarT::serialize(void*, unsigned int) const -00000012 r print_done()::__c -00000012 r select_logs(unsigned char, Menu::arg const*)::__c -00000012 r setup_frame(unsigned char, Menu::arg const*)::__c -00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c -00000012 B adc -00000013 r setup_compass(unsigned char, Menu::arg const*)::__c -00000013 r change_command(unsigned char)::__c -00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c -00000014 t startup_ground() -00000014 W AP_VarT::unserialize(void*, unsigned int) -00000014 W AP_VarT::cast_to_float() const -00000014 W AP_VarT::cast_to_float() const -00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c -00000014 r test_tri(unsigned char, Menu::arg const*)::__c -00000015 r map_baudrate(signed char, unsigned long)::__c -00000015 r init_ardupilot()::__c -00000015 r Log_Read_Motors()::__c -00000015 r print_hit_enter()::__c -00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c -00000016 r init_ardupilot()::__c -00000016 r GCS_MAVLINK::update()::__c -00000016 B sonar -00000017 r __menu_name__main_menu -00000018 t setup_tune(unsigned char, Menu::arg const*) -00000018 W AP_VarT::serialize(void*, unsigned int) const -00000018 b mavlink_get_channel_status::m_mavlink_status -00000019 r GCS_MAVLINK::update()::__c -0000001a r print_log_menu()::__c -0000001c W AP_VarS >::unserialize(void*, unsigned int) -0000001c W AP_VarS >::unserialize(void*, unsigned int) -0000001c W AP_VarT::unserialize(void*, unsigned int) -0000001c W AP_VarS >::serialize(void*, unsigned int) const -0000001c W AP_VarS >::serialize(void*, unsigned int) const -0000001e r Log_Read_Optflow()::__c -0000001e r Log_Read_Nav_Tuning()::__c -0000001f r test_mag(unsigned char, Menu::arg const*)::__c -00000020 r test_current(unsigned char, Menu::arg const*)::__c -00000020 t byte_swap_4 -00000021 r print_log_menu()::__c -00000021 r report_compass()::__c -00000021 r Log_Read_Current()::__c -00000022 t clear_leds() -00000022 t print_blanks(int) -00000022 t reset_hold_I() -00000022 W AP_Float16::~AP_Float16() -00000022 W AP_VarS >::~AP_VarS() -00000022 W AP_VarS >::~AP_VarS() -00000022 W AP_VarT::~AP_VarT() -00000022 W AP_VarT::~AP_VarT() -00000022 W AP_VarT::~AP_VarT() -00000023 r setup_mode(unsigned char, Menu::arg const*)::__c -00000023 r print_gyro_offsets()::__c -00000024 r init_ardupilot()::__c -00000024 r print_accel_offsets()::__c -00000025 r setup_factory(unsigned char, Menu::arg const*)::__c -00000026 t print_done() -00000026 b mavlink_queue -00000026 t print_hit_enter() -00000026 t Log_Read_Startup() -00000027 r test_xbee(unsigned char, Menu::arg const*)::__c -00000028 t test_battery(unsigned char, Menu::arg const*) -00000028 t main_menu_help(unsigned char, Menu::arg const*) -00000028 t help_log(unsigned char, Menu::arg const*) -00000028 W AP_VarT::unserialize(void*, unsigned int) -00000028 W AP_VarT::serialize(void*, unsigned int) const -00000028 r Log_Read_Cmd()::__c -00000028 B imu -00000029 r setup_mode(unsigned char, Menu::arg const*)::__c -00000029 r test_gps(unsigned char, Menu::arg const*)::__c -0000002a t setup_declination(unsigned char, Menu::arg const*) -0000002a r Log_Read_Control_Tuning()::__c -0000002b r planner_mode(unsigned char, Menu::arg const*)::__c -0000002e t print_divider() -0000002e t send_rate(unsigned int, unsigned int) -0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char) -0000002e r Log_Read_Performance()::__c -0000002f r test_radio(unsigned char, Menu::arg const*)::__c -00000030 t planner_mode(unsigned char, Menu::arg const*) -00000032 T GCS_MAVLINK::init(BetterStream*) -00000032 W APM_PI::~APM_PI() -00000034 t _MAV_RETURN_float -00000034 W AP_Float16::serialize(void*, unsigned int) const -00000034 t _mav_put_int8_t_array -00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c -00000036 t report_radio() -00000036 r Log_Read_GPS()::__c -00000037 r print_wp(Location*, unsigned char)::__c -00000038 t init_throttle_cruise() -00000038 r setup_radio(unsigned char, Menu::arg const*)::__c -00000038 r setup_factory(unsigned char, Menu::arg const*)::__c -0000003a t report_gps() -0000003a t report_imu() -0000003a B g_gps_driver -0000003c W RC_Channel::~RC_Channel() -0000003e t verify_RTL() -0000003e T GCS_MAVLINK::send_text(unsigned char, prog_char_t const*) -0000003e W AP_VarT::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char) -00000040 W AP_Float16::unserialize(void*, unsigned int) -00000040 t byte_swap_8 -00000040 t crc_accumulate -00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c -00000044 t setup_show(unsigned char, Menu::arg const*) -00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) -0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) -0000004c t update_auto_yaw() -0000004e T mavlink_send_text(mavlink_channel_t, unsigned char, char const*) -00000050 t report_version() -00000050 r log_menu_commands -00000050 r main_menu_commands -00000050 T GCS_MAVLINK::_find_parameter(unsigned int) -00000052 t change_command(unsigned char) -00000054 t print_enabled(unsigned char) -00000054 t update_motor_leds() -00000054 t report_flight_modes() -00000055 r main_menu_help(unsigned char, Menu::arg const*)::__c -00000055 r setup_flightmodes(unsigned char, Menu::arg const*)::__c -00000056 t readSwitch() -00000056 t dancing_light() -00000057 r help_log(unsigned char, Menu::arg const*)::__c -00000057 B dcm -0000005a t read_control_switch() -0000005c t get_num_logs() -0000005c t setup_esc(unsigned char, Menu::arg const*) -0000005e t update_GPS_light() -0000005e t radio_input_switch() -0000005e T GCS_MAVLINK::_count_parameters() -00000060 t print_switch(unsigned char, unsigned char) -00000060 t _mavlink_send_uart -00000064 t print_gyro_offsets() -00000064 t print_accel_offsets() -00000064 t test_xbee(unsigned char, Menu::arg const*) -00000064 t mavlink_msg_param_value_send -00000068 t zero_eeprom() -00000068 t find_last_log_page(int) -0000006a W GCS_MAVLINK::~GCS_MAVLINK() -0000006c t setup_sonar(unsigned char, Menu::arg const*) -0000006e T output_min() -00000078 t setup_batt_monitor(unsigned char, Menu::arg const*) -0000007a t setup_factory(unsigned char, Menu::arg const*) -0000007e t test_rawgps(unsigned char, Menu::arg const*) -00000080 T __vector_25 -00000080 T __vector_36 -00000080 T __vector_54 -00000082 t do_RTL() -00000086 t Log_Read_Attitude() -00000088 t Log_Read_Raw() -0000008c t setup_frame(unsigned char, Menu::arg const*) -0000008e t dump_log(unsigned char, Menu::arg const*) -00000090 t init_compass() -00000090 t report_tuning() -00000092 t test_tuning(unsigned char, Menu::arg const*) -00000095 r init_ardupilot()::__c -00000096 t map_baudrate(signed char, unsigned long) -00000096 t print_wp(Location*, unsigned char) -0000009a t Log_Read_Motors() -0000009d B gcs -0000009d B hil -0000009e t setup_mode(unsigned char, Menu::arg const*) -0000009e t Log_Read_Mode() -0000009e t Log_Write_Cmd(unsigned char, Location*) -000000a4 T __vector_26 -000000a4 T __vector_37 -000000a4 T __vector_55 -000000a8 t test_sonar(unsigned char, Menu::arg const*) -000000ab B compass -000000ae t report_frame() -000000b2 t erase_logs(unsigned char, Menu::arg const*) -000000b4 t test_relay(unsigned char, Menu::arg const*) -000000b4 t planner_gcs(unsigned char, Menu::arg const*) -000000b6 t get_log_boundaries(unsigned char, int&, int&) -000000be t Log_Read_Nav_Tuning() -000000c2 t test_eedump(unsigned char, Menu::arg const*) -000000c2 t setup_compass(unsigned char, Menu::arg const*) -000000c4 t get_distance(Location*, Location*) -000000c4 t update_events() -000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c -000000c6 t Log_Read(int, int) -000000c6 t test_tri(unsigned char, Menu::arg const*) -000000cc t read_radio() -000000d0 t get_bearing(Location*, Location*) -000000d8 t test_radio(unsigned char, Menu::arg const*) -000000de t Log_Read_Performance() -000000de t Log_Read_Control_Tuning() -000000e0 r setup_menu_commands -000000e0 b mavlink_parse_char::m_mavlink_message -000000e4 t test_radio_pwm(unsigned char, Menu::arg const*) -000000e4 t Log_Read_Optflow() -000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&) -000000e6 t setup_flightmodes(unsigned char, Menu::arg const*) -000000e8 t Log_Read_Current() -000000ee t report_batt_monitor() -000000f4 t _mav_finalize_message_chan_send -000000f6 t Log_Read_Cmd() -000000fa t calc_nav_pitch_roll() -00000100 r test_menu_commands -0000010a t test_gps(unsigned char, Menu::arg const*) -0000010c W RC_Channel::RC_Channel(unsigned int, prog_char_t const*) -00000112 t test_current(unsigned char, Menu::arg const*) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int) -00000118 t set_command_with_index(Location, int) -00000118 T GCS_MAVLINK::_queued_send() -00000126 t arm_motors() -00000128 t get_command_with_index(int) -00000130 t report_compass() -00000134 T GCS_MAVLINK::send_message(unsigned char, unsigned long) -0000014e T GCS_MAVLINK::update() -00000150 t update_trig() -00000152 t set_next_WP(Location*) -00000156 t Log_Read_GPS() -00000166 t select_logs(unsigned char, Menu::arg const*) -0000016c t update_commands() -00000170 t test_mag(unsigned char, Menu::arg const*) -00000172 t update_nav_wp() -000001a0 t init_home() -000001a8 t print_radio_values() -000001b6 t setup_motors(unsigned char, Menu::arg const*) -000001ca t mavlink_delay(unsigned long) -000001ce t start_new_log() -000001ea T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) -00000200 t set_mode(unsigned char) -0000021c t test_wp(unsigned char, Menu::arg const*) -00000228 t setup_radio(unsigned char, Menu::arg const*) -0000029e t test_imu(unsigned char, Menu::arg const*) -000002ea t tuning() -00000330 t calc_nav_rate(int, int, int, int) -00000358 T update_throttle_mode() -00000382 t print_log_menu() -000003be t read_battery() -000003dc T update_yaw_mode() -000004b2 t mavlink_parse_char -00000578 t __static_initialization_and_destruction_0(int, int) -000005ea T update_roll_pitch_mode() -00000616 t init_ardupilot() -000007dc b g -0000083c W Parameters::Parameters() -000008e4 t process_next_command() -00001196 t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001494 T GCS_MAVLINK::handleMessage(__mavlink_message*) -00001900 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt index a48a9b82be..053b0687c4 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -593,14 +592,14 @@ 000001a0 t init_home() 000001a8 t print_radio_values() 000001b6 t setup_motors(unsigned char, Menu::arg const*) +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 00000200 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001e86 T loop +00001e70 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt index b8ece535cd..8adec51a70 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -593,14 +592,14 @@ 000001a0 t init_home() 000001a8 t print_radio_values() 000001b6 t setup_motors(unsigned char, Menu::arg const*) +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 00000200 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001e84 T loop +00001e6e T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt index 02d799cbcd..98da5c5fa8 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -593,14 +592,14 @@ 00000172 t update_nav_wp() 000001a0 t init_home() 000001a8 t print_radio_values() +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 00000200 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001de6 T loop +00001dd0 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt index e093161912..f9e573a410 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -593,14 +592,14 @@ 00000172 t update_nav_wp() 000001a0 t init_home() 000001a8 t print_radio_values() +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 00000200 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001de4 T loop +00001dce T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt index 5d308cf013..3a14243098 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -593,14 +592,14 @@ 00000172 t update_nav_wp() 000001a0 t init_home() 000001a8 t print_radio_values() +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 00000200 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001ec6 T loop +00001eb0 T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt index 98b0c692ae..bd43df11d9 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt @@ -469,7 +469,6 @@ 00000040 t byte_swap_8 00000040 t crc_accumulate 00000042 t report_sonar() -00000043 r test_imu(unsigned char, Menu::arg const*)::__c 00000044 t setup_show(unsigned char, Menu::arg const*) 00000044 W AP_VarT::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char) 0000004a W AP_VarT::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char) @@ -593,14 +592,14 @@ 00000172 t update_nav_wp() 000001a0 t init_home() 000001a8 t print_radio_values() +000001b8 t test_imu(unsigned char, Menu::arg const*) 000001be T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int) +000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -000001e2 t arm_motors() 00000200 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) -00000286 t test_imu(unsigned char, Menu::arg const*) 000002ea t tuning() 00000330 t calc_nav_rate(int, int, int, int) 00000358 T update_throttle_mode() @@ -616,4 +615,4 @@ 000008e4 t process_next_command() 000011d8 T GCS_MAVLINK::handleMessage(__mavlink_message*) 000017ac t mavlink_try_send_message(mavlink_channel_t, unsigned char, unsigned int) -00001ec4 T loop +00001eae T loop diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml index b285a250b5..14b7ec0c09 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml +++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml @@ -58,7 +58,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex - ArduCopter 2.0.41 Beta Quad + ArduCopter 2.0.42 Beta Quad #define FRAME_CONFIG QUAD_FRAME #define FRAME_ORIENTATION X_FRAME @@ -69,7 +69,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex - ArduCopter 2.0.41 Beta Tri + ArduCopter 2.0.42 Beta Tri #define FRAME_CONFIG TRI_FRAME #define FRAME_ORIENTATION X_FRAME @@ -80,7 +80,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex - ArduCopter 2.0.41 Beta Hexa + ArduCopter 2.0.42 Beta Hexa #define FRAME_CONFIG HEXA_FRAME #define FRAME_ORIENTATION X_FRAME @@ -91,7 +91,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex - ArduCopter 2.0.41 Beta Y6 + ArduCopter 2.0.42 Beta Y6 #define FRAME_CONFIG Y6_FRAME #define FRAME_ORIENTATION X_FRAME @@ -102,7 +102,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.hex - ArduCopter 2.0.41 Beta Heli (2560 only) + ArduCopter 2.0.42 Beta Heli (2560 only) #define AUTO_RESET_LOITER 0 #define FRAME_CONFIG HELI_FRAME @@ -149,7 +149,7 @@ http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex - ArduCopter 2.0.41 Beta Quad Hil + ArduCopter 2.0.42 Beta Quad Hil #define FRAME_CONFIG QUAD_FRAME #define FRAME_ORIENTATION PLUS_FRAME diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index d43656d340..b0921e0e03 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1,9 +1,12 @@ From https://code.google.com/p/ardupilot-mega - 22593cc..5e95582 APM_Camera -> origin/APM_Camera - 0448983..dc0031c master -> origin/master -Updating 0448983..dc0031c + 5e95582..28bb6fd APM_Camera -> origin/APM_Camera + 5e48582..45a85df master -> origin/master +Updating 5e48582..45a85df Fast-forward - ArduCopter/ArduCopter.pde | 6 ++++++ - ArduCopter/Parameters.h | 3 +++ - ArduCopter/defines.h | 6 ++++-- - 3 files changed, 13 insertions(+), 2 deletions(-) + ArduCopter/ArduCopter.pde | 13 +++----- + ArduCopter/motors.pde | 9 +++-- + ArduCopter/system.pde | 2 +- + ArduCopter/test.pde | 2 +- + libraries/AP_DCM/AP_DCM.cpp | 71 ++++++++++++++++++++++++++++++++++++++----- + libraries/AP_DCM/AP_DCM.h | 7 ++++- + 6 files changed, 81 insertions(+), 23 deletions(-) From d733868fead7e28eb06e4870238d975baf3c4815 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 11:48:11 -0700 Subject: [PATCH 48/74] Fixed Yaw mode for CIRCLE --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 1b3423e4ca..29514f99c1 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -345,7 +345,7 @@ // CIRCLE Mode #ifndef CIRCLE_YAW -# define CIRCLE_YAW YAW_HOLD +# define CIRCLE_YAW YAW_AUTO #endif #ifndef CIRCLE_RP From 01559f4c2266c1caa91d7c2676919d15a8681448 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Mon, 12 Sep 2011 02:50:53 +0800 Subject: [PATCH 49/74] firmware build --- .../Firmware/AC2-Hexa-1280.size.txt | 2 +- .../Firmware/AC2-Hexa-2560.size.txt | 2 +- .../Firmware/AC2-Quad-1280.size.txt | 2 +- .../Firmware/AC2-Quad-2560.size.txt | 2 +- .../Firmware/AC2-Tri-1280.size.txt | 2 +- .../Firmware/AC2-Tri-2560.size.txt | 2 +- .../Firmware/AC2-Y6-1280.size.txt | 2 +- .../Firmware/AC2-Y6-2560.size.txt | 2 +- Tools/ArdupilotMegaPlanner/Firmware/git.log | 14 ++++---------- 9 files changed, 12 insertions(+), 18 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt index 9ff2b1f235..d84d3fb9ee 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt @@ -597,7 +597,7 @@ 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() 000001fc t setup_motors(unsigned char, Menu::arg const*) -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt index bc16bd7f7a..da96f935e5 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt @@ -597,7 +597,7 @@ 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() 000001fc t setup_motors(unsigned char, Menu::arg const*) -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt index 053b0687c4..c058444df4 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt @@ -597,7 +597,7 @@ 000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt index 8adec51a70..ccc1134565 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt @@ -597,7 +597,7 @@ 000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt index 98da5c5fa8..189044ca08 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt @@ -597,7 +597,7 @@ 000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt index f9e573a410..0a4a5f04fa 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt @@ -597,7 +597,7 @@ 000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt index 3a14243098..1e082c332d 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt @@ -597,7 +597,7 @@ 000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 00000220 t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt index bd43df11d9..09fc850668 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt @@ -597,7 +597,7 @@ 000001ca t arm_motors() 000001ca t mavlink_delay(unsigned long) 000001ce t start_new_log() -00000200 t set_mode(unsigned char) +00000202 t set_mode(unsigned char) 0000021c t test_wp(unsigned char, Menu::arg const*) 00000228 t setup_radio(unsigned char, Menu::arg const*) 000002ea t tuning() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index b0921e0e03..eb1aa83247 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1,12 +1,6 @@ From https://code.google.com/p/ardupilot-mega - 5e95582..28bb6fd APM_Camera -> origin/APM_Camera - 5e48582..45a85df master -> origin/master -Updating 5e48582..45a85df + a543a5c..fe617ae master -> origin/master +Updating a543a5c..fe617ae Fast-forward - ArduCopter/ArduCopter.pde | 13 +++----- - ArduCopter/motors.pde | 9 +++-- - ArduCopter/system.pde | 2 +- - ArduCopter/test.pde | 2 +- - libraries/AP_DCM/AP_DCM.cpp | 71 ++++++++++++++++++++++++++++++++++++++----- - libraries/AP_DCM/AP_DCM.h | 7 ++++- - 6 files changed, 81 insertions(+), 23 deletions(-) + ArduCopter/config.h | 2 +- + 1 files changed, 1 insertions(+), 1 deletions(-) From ce010b4e38d33d8befaed8e89967bb88afcbf4b2 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 23:07:30 +0200 Subject: [PATCH 50/74] Moved update_aux_servo_function() to the RC_Channel_aux.* files. This simplifies code sharing between ArduCopter and Arduplane at the expense of 48bytes. Moved CH_x defines out of the defines.h file and into the library where they belong --- ArduPlane/ArduPlane.pde | 2 +- ArduPlane/defines.h | 13 ---------- ArduPlane/radio.pde | 33 +++---------------------- libraries/APM_RC/APM_RC.h | 11 +++++++++ libraries/RC_Channel/RC_Channel_aux.cpp | 27 ++++++++++++++++++++ libraries/RC_Channel/RC_Channel_aux.h | 4 ++- 6 files changed, 46 insertions(+), 44 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 02884aab63..d802efe651 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -735,7 +735,7 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); - update_aux_servo_function(); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); #if CAMERA == ENABLED camera_mount.update_mount_type(); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index fff75fda4f..5c5b01b10f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,19 +41,6 @@ #define GPS_PROTOCOL_MTK16 6 #define GPS_PROTOCOL_AUTO 7 -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 4f7292879d..6311f63e3e 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -195,7 +195,7 @@ static void trim_control_surfaces() g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel }else{ elevon1_trim = ch1_temp; @@ -212,7 +212,7 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } static void trim_radio() @@ -228,7 +228,7 @@ static void trim_radio() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -244,30 +244,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); -} - -// update the g_rc_function array from pointers to rc_x channels -// This should be done periodically because the user might change the configuration and -// expects the changes to take effect instantly -static void update_aux_servo_function() -{ - // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function - RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos - aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)g.rc_5.function.get(); - aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)g.rc_6.function.get(); - aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)g.rc_7.function.get(); - aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)g.rc_8.function.get(); - - // Assume that no auxiliary function is used - for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) - { - g_rc_function[i] = NULL; - } - - // assign the RC channel to each function - g_rc_function[aux_servo_function[CH_5]] = &g.rc_5; - g_rc_function[aux_servo_function[CH_6]] = &g.rc_6; - g_rc_function[aux_servo_function[CH_7]] = &g.rc_7; - g_rc_function[aux_servo_function[CH_8]] = &g.rc_8; + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 236f85593d..bcac692abe 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,6 +5,17 @@ #define MIN_PULSEWIDTH 900 #define MAX_PULSEWIDTH 2100 +// Radio channels +// Note channels are from 0! +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + #include class APM_RC_Class diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index f6ceb5fed5..caa7affb61 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -3,6 +3,8 @@ #include #include "RC_Channel_aux.h" +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function + int16_t RC_Channel_aux::closest_limit(int16_t angle) { @@ -67,3 +69,28 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) APM_RC.OutputCh(ch_nr, radio_out); } + +// update the g_rc_function array from pointers to rc_x channels +// This should be done periodically because the user might change the configuration and +// expects the changes to take effect instantly +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8) +{ + // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function + RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos + aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)rc_5->function.get(); + aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)rc_6->function.get(); + aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)rc_7->function.get(); + aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)rc_8->function.get(); + + // Assume that no auxiliary function is used + for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) + { + g_rc_function[i] = NULL; + } + + // assign the RC channel to each function + g_rc_function[aux_servo_function[CH_5]] = rc_5; + g_rc_function[aux_servo_function[CH_6]] = rc_6; + g_rc_function[aux_servo_function[CH_7]] = rc_7; + g_rc_function[aux_servo_function[CH_8]] = rc_8; +} diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 7e7fd4f167..8fae791fdc 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /// @file RC_Channel_aux.h -/// @brief RC_Channel manager, with EEPROM-backed storage of constants. +/// @brief RC_Channel manager for Channels 5..8, with EEPROM-backed storage of constants. /// @author Amilcar Lucas #ifndef RC_CHANNEL_AUX_H_ @@ -53,4 +53,6 @@ public: }; +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8); + #endif /* RC_CHANNEL_AUX_H_ */ From 46f9d4cec85d2ae82d5ed30ebb1a4c08f71a243c Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 23:25:06 +0200 Subject: [PATCH 51/74] This is ugly, but it fixes compilation on arduino --- ArduPlane/ArduPlane.pde | 2 +- libraries/RC_Channel/RC_Channel.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d802efe651..3b62fe925a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -39,7 +39,7 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library -#include <../libraries/RC_Channel/RC_Channel_aux.h> // Auxiliary RC Channel Library (Channels 5..8) +//#include // Auxiliary RC Channel Library (Channels 5..8) #include // Range finder library #include #include // Photo or video camera diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index d5f721b4fc..4ff5990bbf 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -102,4 +102,6 @@ class RC_Channel{ int16_t _low; }; +#include "RC_Channel_aux.h" + #endif From 8b3e9d2df6e5dddb52f1279016b55768a0c08851 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sun, 11 Sep 2011 23:32:04 +0200 Subject: [PATCH 52/74] Signed-off-by: Olivier ADLER slightly upped motor out values for motor test. Was too low for some ESCs. --- ArduCopter/motors_hexa.pde | 36 ++++++++++++++++++------------------ ArduCopter/motors_octa.pde | 16 ++++++++-------- ArduCopter/motors_quad.pde | 24 ++++++++++++------------ ArduCopter/motors_tri.pde | 6 +++--- ArduCopter/motors_y6.pde | 12 ++++++------ 5 files changed, 47 insertions(+), 47 deletions(-) diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index d1dde00eee..066e7c2d17 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -138,21 +138,21 @@ static void output_motor_test() // 31 // 24 if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 50; - motor_out[CH_4] += 50; + motor_out[CH_8] += 100; + motor_out[CH_4] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[CH_7] += 50; - motor_out[CH_3] += 50; + motor_out[CH_7] += 100; + motor_out[CH_3] += 100; } }else{ @@ -160,21 +160,21 @@ static void output_motor_test() // 2 1 // 4 if(g.rc_1.control_in > 3000){ // right - motor_out[CH_4] += 50; - motor_out[CH_8] += 50; + motor_out[CH_4] += 100; + motor_out[CH_8] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_7] += 50; - motor_out[CH_3] += 50; + motor_out[CH_7] += 100; + motor_out[CH_3] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; } } @@ -189,27 +189,27 @@ static void output_motor_test() /* APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); delay(1000); } */ diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index c798014c16..d195b8ad61 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -183,35 +183,35 @@ static void output_motors_disarmed() static void output_motor_test() { APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); delay(1000); APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50); + APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); delay(1000); } diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index dc6b36ee06..d5af159ad4 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -139,23 +139,23 @@ static void output_motor_test() // 31 // 24 if(g.rc_1.control_in > 3000){ - motor_out[CH_1] += 50; - motor_out[CH_4] += 50; + motor_out[CH_1] += 100; + motor_out[CH_4] += 100; } if(g.rc_1.control_in < -3000){ - motor_out[CH_2] += 50; - motor_out[CH_3] += 50; + motor_out[CH_2] += 100; + motor_out[CH_3] += 100; } if(g.rc_2.control_in > 3000){ - motor_out[CH_2] += 50; - motor_out[CH_4] += 50; + motor_out[CH_2] += 100; + motor_out[CH_4] += 100; } if(g.rc_2.control_in < -3000){ - motor_out[CH_1] += 50; - motor_out[CH_3] += 50; + motor_out[CH_1] += 100; + motor_out[CH_3] += 100; } }else{ @@ -163,16 +163,16 @@ static void output_motor_test() // 2 1 // 4 if(g.rc_1.control_in > 3000) - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; if(g.rc_1.control_in < -3000) - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; if(g.rc_2.control_in > 3000) - motor_out[CH_4] += 50; + motor_out[CH_4] += 100; if(g.rc_2.control_in < -3000) - motor_out[CH_3] += 50; + motor_out[CH_3] += 100; } APM_RC.OutputCh(CH_1, motor_out[CH_1]); diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde index 0fbff12ab7..ec9c747b41 100644 --- a/ArduCopter/motors_tri.pde +++ b/ArduCopter/motors_tri.pde @@ -96,15 +96,15 @@ static void output_motor_test() if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 50; + motor_out[CH_1] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 50; + motor_out[CH_2] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_4] += 50; + motor_out[CH_4] += 100; } APM_RC.OutputCh(CH_1, motor_out[CH_1]); diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index ede41a0fba..23f51218a2 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -144,18 +144,18 @@ static void output_motor_test() if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 50; - motor_out[CH_7] += 50; + motor_out[CH_1] += 100; + motor_out[CH_7] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 50; - motor_out[CH_3] += 50; + motor_out[CH_2] += 100; + motor_out[CH_3] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 50; - motor_out[CH_4] += 50; + motor_out[CH_8] += 100; + motor_out[CH_4] += 100; } APM_RC.OutputCh(CH_1, motor_out[CH_1]); From 77c798abd5480e6dc2843563b34ad819a403b087 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 00:02:47 +0200 Subject: [PATCH 53/74] Use G_RC_AUX macro to simplify accessing the auxiliary servos --- ArduPlane/radio.pde | 48 ++++++++------------------- libraries/AP_Camera/AP_Camera.cpp | 5 +-- libraries/AP_DCM/AP_DCM.h | 2 +- libraries/AP_Mount/AP_Mount.cpp | 9 ++--- libraries/RC_Channel/RC_Channel.h | 1 + libraries/RC_Channel/RC_Channel_aux.h | 3 ++ 6 files changed, 23 insertions(+), 45 deletions(-) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 6311f63e3e..ed3d1782b2 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -23,46 +23,26 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - if (g_rc_function[RC_Channel_aux::k_flap]) { - g_rc_function[RC_Channel_aux::k_flap]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_flap_auto]) { - g_rc_function[RC_Channel_aux::k_flap_auto]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_aileron]) { - g_rc_function[RC_Channel_aux::k_aileron]->set_angle(SERVO_MAX); - } - if (g_rc_function[RC_Channel_aux::k_flaperon]) { - g_rc_function[RC_Channel_aux::k_flaperon]->set_range(0,100); - } + G_RC_AUX(k_flap)->set_range(0,100); + G_RC_AUX(k_flap_auto)->set_range(0,100); + G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); + G_RC_AUX(k_flaperon)->set_range(0,100); #if CAMERA == ENABLED - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) { - g_rc_function[RC_Channel_aux::k_mount_yaw]->set_range( + G_RC_AUX(k_mount_yaw)->set_range( g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) { - g_rc_function[RC_Channel_aux::k_mount_pitch]->set_range( + G_RC_AUX(k_mount_pitch)->set_range( g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_mount_roll]) { - g_rc_function[RC_Channel_aux::k_mount_roll]->set_range( + G_RC_AUX(k_mount_roll)->set_range( g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) { - g_rc_function[RC_Channel_aux::k_cam_trigger]->set_range( + G_RC_AUX(k_cam_trigger)->set_range( g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_cam_open]) { - g_rc_function[RC_Channel_aux::k_cam_open]->set_range(0,100); - } + G_RC_AUX(k_cam_open)->set_range(0,100); #endif - if (g_rc_function[RC_Channel_aux::k_egg_drop]) { - g_rc_function[RC_Channel_aux::k_egg_drop]->set_range(0,100); - } + G_RC_AUX(k_egg_drop)->set_range(0,100); } static void init_rc_out() @@ -195,7 +175,7 @@ static void trim_control_surfaces() g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel }else{ elevon1_trim = ch1_temp; @@ -212,7 +192,7 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } static void trim_radio() @@ -228,7 +208,7 @@ static void trim_radio() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -244,5 +224,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 6fed9b86cb..ddfbc2d8a3 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -98,10 +98,7 @@ AP_Camera::trigger_pic_cleanup() case 0: case 2: case 3: - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) - { - g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; - } + G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; break; case 1: // TODO for some strange reason the function call bellow gives a linker error diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index bd705da8f8..6537c507a0 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef AP_DCM_h #define AP_DCM_h -// teporarily include all other classes here +// temporarily include all other classes here // since this naming is a bit off from the // convention and the AP_DCM should be the top // header file diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6cf76c9351..268533ee3b 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -156,12 +156,9 @@ void AP_Mount::update_mount() // write the results to the servos // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - if (g_rc_function[RC_Channel_aux::k_mount_roll]) - g_rc_function[RC_Channel_aux::k_mount_roll]->closest_limit(roll_angle/10); - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) - g_rc_function[RC_Channel_aux::k_mount_pitch]->closest_limit(pitch_angle/10); - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) - g_rc_function[RC_Channel_aux::k_mount_yaw]->closest_limit(yaw_angle/10); + G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); + G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); + G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); } void AP_Mount::set_mode(MountMode mode) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 4ff5990bbf..132e9146f3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -102,6 +102,7 @@ class RC_Channel{ int16_t _low; }; +// This is ugly, but it fixes compilation on arduino #include "RC_Channel_aux.h" #endif diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 8fae791fdc..6d8c228472 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -9,6 +9,9 @@ #include "RC_Channel.h" +// Macro to simplify accessing the auxiliary servos +#define G_RC_AUX(_t) if (g_rc_function[RC_Channel_aux::_t]) g_rc_function[RC_Channel_aux::_t] + /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function /// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements From b9383537e2cd70bc89d3806fe0e1d4f3c6ec5e7f Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Mon, 12 Sep 2011 00:43:54 +0200 Subject: [PATCH 54/74] Added missing motor tests for octa quad and octa V --- ArduCopter/motors_octa_quad.pde | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index ac40870ab0..7866c7d3ab 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -183,4 +183,3 @@ static void output_motor_test() } #endif - From 466488e78d0ca0b1bd073f37d1a285681b481a16 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:27:56 -0700 Subject: [PATCH 55/74] removed nav_bearing - not used --- ArduCopter/commands.pde | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 10d5fb31f3..7984e7c6c7 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -190,7 +190,6 @@ static void set_next_WP(struct Location *wp) wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; target_bearing = get_bearing(¤t_loc, &next_WP); - nav_bearing = target_bearing; // to check if we have missed the WP // ---------------------------- From c15b6be83f746f7b49f1ef9b5711d4a73521547b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:28:43 -0700 Subject: [PATCH 56/74] increased imax --- ArduCopter/config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 29514f99c1..144c8f650d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -482,10 +482,10 @@ # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch #endif #ifndef NAV_I -# define NAV_I 0.1 // this +# define NAV_I 0.12 // this #endif #ifndef NAV_IMAX -# define NAV_IMAX 16 // degrees +# define NAV_IMAX 20 // degrees #endif /* From 5f94aa1d40a6c4f9394c67a465d12bb2c6101e04 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:32:58 -0700 Subject: [PATCH 57/74] removed Nav_bearing --- ArduCopter/Log.pde | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d67fabf2a8..9f1ef3a578 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -537,9 +537,8 @@ static void Log_Write_Nav_Tuning() DataFlash.WriteInt((int)wp_distance); // 1 DataFlash.WriteByte(wp_verify_byte); // 2 DataFlash.WriteInt((int)(target_bearing/100)); // 3 - DataFlash.WriteInt((int)(nav_bearing/100)); // 4 - DataFlash.WriteInt((int)long_error); // 5 - DataFlash.WriteInt((int)lat_error); // 6 + DataFlash.WriteInt((int)long_error); // 4 + DataFlash.WriteInt((int)lat_error); // 5 /* @@ -568,7 +567,6 @@ static void Log_Read_Nav_Tuning() DataFlash.ReadInt(), // distance DataFlash.ReadByte(), // wp_verify_byte DataFlash.ReadInt(), // target_bearing - DataFlash.ReadInt(), // nav_bearing DataFlash.ReadInt(), // long_error DataFlash.ReadInt()); // lat_error From 285029856b351fd53b2bf3d62e5b7d02ee39a131 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:33:29 -0700 Subject: [PATCH 58/74] removed nav_bearing --- ArduCopter/Mavlink_Common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index 7d54f4ef80..a04ffecd4c 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -142,7 +142,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ chan, nav_roll / 1.0e2, nav_pitch / 1.0e2, - nav_bearing / 1.0e2, + target_bearing / 1.0e2, target_bearing / 1.0e2, wp_distance, altitude_error / 1.0e2, From bae05178f068472b02b78b8c95fa399df16ea948 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:36:20 -0700 Subject: [PATCH 59/74] removed Xtrack and increased rate error limit --- ArduCopter/navigation.pde | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 6cddf96010..3b78dadf90 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -34,11 +34,12 @@ static void navigate() // nav_bearing will include xtrac correction // ----------------------------------------- //xtrack_enabled = false; - if(xtrack_enabled){ - nav_bearing = wrap_360(target_bearing + get_crosstrack_correction()); - }else{ - nav_bearing = target_bearing; - } + + //if(xtrack_enabled){ + // crosstrack_correction = get_crosstrack_correction(); + //}else { + // crosstrack_correction = 0; + //} } static bool check_missed_wp() @@ -101,18 +102,20 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed } // find the rates: + float temp = radians((float)g_gps->ground_course/100.0); + // calc the cos of the error to tell how fast we are moving towards the target in cm - y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); y_rate_error = y_target_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum + y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); // calc the sin of the error to tell how fast we are moving laterally to the target in cm - x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); + x_actual_speed = (float)g_gps->ground_speed * sin(temp); x_rate_error = x_target_speed - x_actual_speed; - x_rate_error = constrain(x_rate_error, -250, 250); + x_rate_error = constrain(x_rate_error, -600, 600); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); } @@ -128,11 +131,11 @@ static void calc_nav_pitch_roll() } // ------------------------------ -static void calc_bearing_error() +/*static void calc_bearing_error() { bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = wrap_180(bearing_error); -} +}*/ static long get_altitude_error() { @@ -189,6 +192,7 @@ static long wrap_180(long error) return error; } +/* static long get_crosstrack_correction(void) { // Crosstrack Error @@ -206,7 +210,7 @@ static long get_crosstrack_correction(void) } return 0; } - +*/ static long cross_track_test() { From 885ec18182e7e79b93e46f4f24107ca9b1478c2c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:40:05 -0700 Subject: [PATCH 60/74] Removed last of Xtrack --- ArduCopter/ArduCopter.pde | 18 ++++++++++-------- ArduCopter/HIL_Xplane.pde | 2 +- ArduCopter/commands.pde | 2 +- ArduCopter/navigation.pde | 7 ++++--- ArduCopter/system.pde | 10 +++++----- 5 files changed, 21 insertions(+), 18 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 541a1ec586..d377ee26bf 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -316,11 +316,13 @@ static bool did_ground_start = false; // have we ground started after first ar // --------------------- static const float radius_of_earth = 6378100; // meters static const float gravity = 9.81; // meters/ sec^2 -static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +//static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -static bool xtrack_enabled = false; -static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +//static bool xtrack_enabled = false; +//static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +//static long crosstrack_correction; // deg * 100 : 0 to 360 desired angle of plane to target + static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static long circle_angle = 0; static byte wp_control; // used to control - navgation or loiter @@ -349,7 +351,7 @@ static int airspeed; // m/s * 100 // Location Errors // --------------- -static long bearing_error; // deg * 100 : 0 to 36000 +//static long bearing_error; // deg * 100 : 0 to 36000 static long altitude_error; // meters * 100 we are off in altitude static long old_altitude; static long yaw_error; // how off are we pointed @@ -674,8 +676,8 @@ static void medium_loop() // ------------------------------------------------------ navigate(); - // control mode specific updates to nav_bearing - // -------------------------------------------- + // control mode specific updates + // ----------------------------- update_navigation(); if (g.log_bitmask & MASK_LOG_NTUN) @@ -799,7 +801,7 @@ static void fifty_hz_loop() #endif // use Yaw to find our bearing error - calc_bearing_error(); + //calc_bearing_error(); //if (throttle_slew < 0) // throttle_slew++; @@ -1246,7 +1248,7 @@ static void update_navigation() wp_control = WP_MODE; }else{ set_mode(LOITER); - xtrack_enabled = false; + //xtrack_enabled = false; } diff --git a/ArduCopter/HIL_Xplane.pde b/ArduCopter/HIL_Xplane.pde index 8f9a748e74..311b9da649 100644 --- a/ArduCopter/HIL_Xplane.pde +++ b/ArduCopter/HIL_Xplane.pde @@ -12,7 +12,7 @@ void HIL_XPLANE::output_HIL(void) output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5 output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7 output_int((int)wp_distance); // 4 bytes 8,9 - output_int((int)bearing_error); // 5 bytes 10,11 + //output_int((int)bearing_error); // 5 bytes 10,11 output_int((int)altitude_error); // 6 bytes 12, 13 output_int((int)energy_error); // 7 bytes 14,15 output_byte((int)g.waypoint_index); // 8 bytes 16 diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 7984e7c6c7..427fc04b6c 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -197,7 +197,7 @@ static void set_next_WP(struct Location *wp) // set a new crosstrack bearing // ---------------------------- - crosstrack_bearing = target_bearing; // Used for track following + //crosstrack_bearing = target_bearing; // Used for track following gcs.print_current_waypoints(); } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 3b78dadf90..a40ef79f27 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -211,18 +211,19 @@ static long get_crosstrack_correction(void) return 0; } */ - +/* static long cross_track_test() { long temp = wrap_180(target_bearing - crosstrack_bearing); return abs(temp); } - +*/ +/* static void reset_crosstrack() { crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } - +*/ static long get_altitude_above_home(void) { // This is the altitude above the home location diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 70daa8e178..f9ce432993 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands); +MENU(main_menu, "ArduCopter 2.0.43 Beta", main_menu_commands); #endif // CLI_ENABLED @@ -362,7 +362,7 @@ static void set_mode(byte mode) led_mode = NORMAL_LEDS; // most modes do not calculate crosstrack correction - xtrack_enabled = false; + //xtrack_enabled = false; reset_nav_I(); switch(control_mode) @@ -411,7 +411,7 @@ static void set_mode(byte mode) // do crosstrack correction // XXX move to flight commands - xtrack_enabled = true; + //xtrack_enabled = true; break; case CIRCLE: @@ -437,7 +437,7 @@ static void set_mode(byte mode) roll_pitch_mode = ROLL_PITCH_AUTO; throttle_mode = THROTTLE_AUTO; - xtrack_enabled = true; + //xtrack_enabled = true; init_throttle_cruise(); next_WP = current_loc; break; @@ -447,7 +447,7 @@ static void set_mode(byte mode) roll_pitch_mode = RTL_RP; throttle_mode = RTL_THR; - xtrack_enabled = true; + //xtrack_enabled = true; init_throttle_cruise(); do_RTL(); break; From 251c5875fbda693367423fe7f818d3aedd037f86 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 21:50:32 -0600 Subject: [PATCH 61/74] Disable stick mixing if in failsafe If trim values differ from failsafe channel values then stick mixing will adversely affect performance while in failsafe --- ArduPlane/Attitude.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 07300a65b6..63eefb30f5 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -61,7 +61,7 @@ static void stabilize() // Mix Stick input to allow users to override control surfaces // ----------------------------------------------------------- - if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { + if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) { // TODO: use RC_Channel control_mix function? @@ -92,7 +92,7 @@ static void stabilize() // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes // important for steering on the ground during landing // ----------------------------------------------- - if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { + if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) { ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; ch4_inf = fabs(ch4_inf); ch4_inf = min(ch4_inf, 400.0); From 8c9757a8d19d4ca96748fa23af254bb85ff83c42 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 21:59:46 -0600 Subject: [PATCH 62/74] Correct state machine processing for long failsafe event following short failsafe from lower modes --- ArduPlane/events.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index acd60e9cfc..faa0e0dc69 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -43,12 +43,12 @@ static void failsafe_long_on_event() case STABILIZE: case FLY_BY_WIRE_A: // middle position case FLY_BY_WIRE_B: // middle position + case CIRCLE: set_mode(RTL); break; case AUTO: case LOITER: - case CIRCLE: if(g.long_fs_action == 1) { set_mode(RTL); } From c1460add3bc754525a58a913769a7a08d5993841 Mon Sep 17 00:00:00 2001 From: Mike Smith Date: Sun, 11 Sep 2011 22:20:54 -0700 Subject: [PATCH 63/74] Reinstate a Cygwin workround that was backed out by the previous changes. Not all platforms support (or require) the -r argument to sed. --- config.mk | 2 ++ libraries/AP_Common/Arduino.mk | 7 ++++++- 2 files changed, 8 insertions(+), 1 deletion(-) create mode 100644 config.mk diff --git a/config.mk b/config.mk new file mode 100644 index 0000000000..8375e45334 --- /dev/null +++ b/config.mk @@ -0,0 +1,2 @@ +BOARD=mega +PORT=/dev/null diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index ae3f3e572e..287684a060 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -259,7 +259,12 @@ SKETCHCPP_SRC := $(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS) # make. # SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p' -LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) +ifneq ($(findstring CYGWIN, $(SYSTYPE)),) + # Workaround a cygwin issue + LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) +else + LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR))) +endif # # Find sketchbook libraries referenced by the sketch. From 119d006e0343ced4c050ef000f92e5961c6d395f Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Mon, 12 Sep 2011 07:24:52 -0600 Subject: [PATCH 64/74] Change default setting for auto_trim to disabled per user (JB) request. --- ArduPlane/APM_Config.h.reference | 4 ++-- ArduPlane/config.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 3786839dc6..4c560ef062 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -398,9 +398,9 @@ // also means that you should avoid switching out of MANUAL while you have // any control stick deflection. // -// The default is to enable AUTO_TRIM. +// The default is to disable AUTO_TRIM. // -//#define AUTO_TRIM ENABLED +//#define AUTO_TRIM DISABLED // ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/config.h b/ArduPlane/config.h index f5fac05daa..d48b4cb73d 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -300,7 +300,7 @@ // AUTO_TRIM // #ifndef AUTO_TRIM -# define AUTO_TRIM ENABLED +# define AUTO_TRIM DISABLED #endif From f05f56f83f6e01855efef5baa59cf49b29b06258 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 17:37:42 -0600 Subject: [PATCH 65/74] Correct bug in initialization of auxiliary channels The value of aux_servo_function[CH_x] was not being set before the radio init_rc_in() function was setting channel angle/range. --- ArduPlane/radio.pde | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index ed3d1782b2..5b26479ce6 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -23,6 +23,7 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); G_RC_AUX(k_flap)->set_range(0,100); G_RC_AUX(k_flap_auto)->set_range(0,100); G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); From c7b3bfd295aecbdb5841899b7985328697431b3c Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 20:03:44 +0200 Subject: [PATCH 66/74] Use the G_RC_AUX macro when possible. Added more comments. Remove unused code --- ArduPlane/ArduPlane.pde | 1 - ArduPlane/Attitude.pde | 12 ++++++------ libraries/RC_Channel/RC_Channel_aux.cpp | 20 +++++--------------- 3 files changed, 11 insertions(+), 22 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3b62fe925a..a01fd6501f 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -39,7 +39,6 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library -//#include // Auxiliary RC Channel Library (Channels 5..8) #include // Range finder library #include #include // Photo or video camera diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 63eefb30f5..4847f86e76 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -274,14 +274,14 @@ static void set_servos(void) } g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_rudder.radio_out = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; + G_RC_AUX(k_aileron)->radio_out = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; } else { if (g.mix_mode == 0) { g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); g.channel_rudder.calc_pwm(); - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) { + if (g_rc_function[RC_Channel_aux::k_aileron]) { g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out; g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); } @@ -315,7 +315,7 @@ static void set_servos(void) } if(control_mode <= FLY_BY_WIRE_B) { - if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; + G_RC_AUX(k_flap_auto)->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_in; } else if (control_mode >= FLY_BY_WIRE_C) { if (g.airspeed_enabled == true) { flapSpeedSource = g.airspeed_cruise; @@ -323,11 +323,11 @@ static void set_servos(void) flapSpeedSource = g.throttle_cruise; } if ( flapSpeedSource > g.flap_1_speed) { - if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = 0; + G_RC_AUX(k_flap_auto)->servo_out = 0; } else if (flapSpeedSource > g.flap_2_speed) { - if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_1_percent; + G_RC_AUX(k_flap_auto)->servo_out = g.flap_1_percent; } else { - if (g_rc_function[RC_Channel_aux::k_flap_auto] != NULL) g_rc_function[RC_Channel_aux::k_flap_auto]->servo_out = g.flap_2_percent; + G_RC_AUX(k_flap_auto)->servo_out = g.flap_2_percent; } } diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index caa7affb61..55ed6819af 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -21,6 +21,7 @@ RC_Channel_aux::closest_limit(int16_t angle) while (min >= 1800) min -= 3600; while (max < -1800) max += 3600; while (max >= 1800) max -= 3600; + // This is done every time because the user might change the min, max values on the fly set_range(min, max); // If the angle is outside servo limits, saturate the angle to the closest limit @@ -44,6 +45,7 @@ RC_Channel_aux::closest_limit(int16_t angle) void RC_Channel_aux::output_ch(unsigned char ch_nr) { + // take care or two corner cases switch(function) { case k_none: // disabled @@ -52,26 +54,14 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) case k_manual: // manual radio_out = radio_in; break; - case k_flap: // flaps - case k_flap_auto: // flaps automated - case k_aileron: // aileron - case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) - case k_mount_yaw: // mount yaw (pan) - case k_mount_pitch: // mount pitch (tilt) - case k_mount_roll: // mount roll - case k_cam_trigger: // camera trigger - case k_cam_open: // camera open - case k_egg_drop: // egg drop - case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning - default: - break; } APM_RC.OutputCh(ch_nr, radio_out); } -// update the g_rc_function array from pointers to rc_x channels -// This should be done periodically because the user might change the configuration and +// Update the g_rc_function array of pointers to rc_x channels +// This is to be done before rc_init so that the channels get correctly initialized. +// It also should be called periodically because the user might change the configuration and // expects the changes to take effect instantly void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8) { From c0b4cb661fd20d067ce784a7fa34b43f17a954d3 Mon Sep 17 00:00:00 2001 From: DrZiplok Date: Mon, 12 Sep 2011 18:43:31 +0000 Subject: [PATCH 67/74] GNU sed wants -r, Darwin/BSD sed wants -E. --- libraries/AP_Common/Arduino.mk | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index 287684a060..f84f06a237 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -259,11 +259,10 @@ SKETCHCPP_SRC := $(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS) # make. # SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p' -ifneq ($(findstring CYGWIN, $(SYSTYPE)),) - # Workaround a cygwin issue - LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) -else +ifeq ($(SYSTYPE),Darwin) LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR))) +else + LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) endif # From f40051a0ce28c7a00976168921b7252033f782d2 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 21:24:54 +0200 Subject: [PATCH 68/74] Remove file added by accident --- config.mk | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 config.mk diff --git a/config.mk b/config.mk deleted file mode 100644 index 8375e45334..0000000000 --- a/config.mk +++ /dev/null @@ -1,2 +0,0 @@ -BOARD=mega -PORT=/dev/null From d1a63db7670d3ac4bd06e073e862cb9389644b81 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 21:32:05 +0200 Subject: [PATCH 69/74] These defines got moved to the library --- ArduCopter/defines.h | 8 -------- 1 file changed, 8 deletions(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 5a0dcbfd91..9bd0fb9f88 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -87,14 +87,6 @@ // Note channels are from 0! // // XXX these should be CH_n defines from RC.h at some point. -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 #define CH_10 9 //PB5 #define CH_11 10 //PE3 From ecd14f4425dc3d084c1182ef8e3e89dca6896b32 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 12 Sep 2011 12:57:36 -0700 Subject: [PATCH 70/74] Cleaned up guided mode Reduced ADC filter to 3 from 6 to increase speed of filter. --- ArduCopter/ArduCopter.pde | 1 - ArduCopter/commands.pde | 43 ++++++++++++++++--------------- ArduCopter/commands_logic.pde | 11 +++----- ArduCopter/system.pde | 4 ++- libraries/AP_ADC/AP_ADC_ADS7844.h | 2 +- 5 files changed, 30 insertions(+), 31 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d377ee26bf..6ad4e24031 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1464,7 +1464,6 @@ static void tuning(){ static void update_nav_wp() { - // XXX Guided mode!!! if(wp_control == LOITER_MODE){ // calc a pitch to the target diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 427fc04b6c..ec1da8f3ff 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -1,5 +1,18 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/*static void init_auto() +{ + //if (g.waypoint_index == g.waypoint_total) { + // do_RTL(); + //} + + // initialize commands + // ------------------- + init_commands(); +} +*/ + static void init_commands() { // zero is home, but we always load the next command (1), in the code. @@ -20,18 +33,6 @@ static void clear_command_queue(){ next_command.id = NO_COMMAND; } -static void init_auto() -{ - //if (g.waypoint_index == g.waypoint_total) { - // Serial.println("ia_f"); - // do_RTL(); - //} - - // initialize commands - // ------------------- - init_commands(); -} - // Getters // ------- static struct Location get_command_with_index(int i) @@ -41,8 +42,6 @@ static struct Location get_command_with_index(int i) // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- if (i > g.waypoint_total) { - Serial.println("XCD"); - // we do not have a valid command to load // return a WP with a "Blank" id temp.id = CMD_BLANK; @@ -51,7 +50,6 @@ static struct Location get_command_with_index(int i) return temp; }else{ - //Serial.println("LD"); // we can load a command, we don't process it yet // read WP position long mem = (WP_START_BYTE) + (i * WP_SIZE); @@ -75,10 +73,9 @@ static struct Location get_command_with_index(int i) } // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative - if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ + //if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ //temp.alt += home.alt; - } - //Serial.println("ADD ALT"); + //} if(temp.options & WP_OPTION_RELATIVE){ // If were relative, just offset from home @@ -217,14 +214,12 @@ static void init_home() home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 //home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid - home.alt = 0; // this is a test + home.alt = 0; // Home is always 0 home_is_set = true; // to point yaw towards home until we set it with Mavlink target_WP = home; - //Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt); - // Save Home to EEPROM // ------------------- // no need to save this to EPROM @@ -233,8 +228,14 @@ static void init_home() // Save prev loc this makes the calcs look better before commands are loaded prev_WP = home; + // this is dangerous since we can get GPS lock at any time. //next_WP = home; + + // Load home for a default guided_WP + // ------------- + guided_WP = home; + guided_WP.alt += g.RTL_altitude; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index f544719c45..68dbce1c55 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -105,17 +105,14 @@ static void handle_process_now() static void handle_no_commands() { - // we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous. - // use landing commands /* switch (control_mode){ default: - //set_mode(RTL); + set_mode(RTL); break; - } - return; - */ - Serial.println("Handle No CMDs"); + }*/ + //return; + //Serial.println("Handle No CMDs"); } /********************************************************************************/ diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index f9ce432993..6eb3c83e8f 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -407,7 +407,8 @@ static void set_mode(byte mode) init_throttle_cruise(); // loads the commands from where we left off - init_auto(); + //init_auto(); + init_commands(); // do crosstrack correction // XXX move to flight commands @@ -440,6 +441,7 @@ static void set_mode(byte mode) //xtrack_enabled = true; init_throttle_cruise(); next_WP = current_loc; + set_next_WP(&guided_WP); break; case RTL: diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 532dfd97de..ca748e4e1f 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -9,7 +9,7 @@ #define ADC_DATAIN 50 // MISO #define ADC_SPICLOCK 52 // SCK #define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40 -#define ADC_FILTER_SIZE 6 +#define ADC_FILTER_SIZE 3 #include "AP_ADC.h" #include From 54069918310c8e4b1832fef57c911e9cf0f012b3 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 01:24:06 +0200 Subject: [PATCH 71/74] Added support for routing any function to any of the aux. servos. This is a manual merge from the APM_Camera branch. It reverts the stuff that Oliver did not menat to do with his commit 6dcbc7f44bc0 --- ArduPlane/APM_Config.h | 16 +- ArduPlane/APM_Config.h.reference | 13 ++ ArduPlane/ArduPlane.pde | 31 +--- ArduPlane/Parameters.h | 39 +++-- ArduPlane/WP_activity.pde | 56 ------- ArduPlane/config.h | 13 ++ ArduPlane/defines.h | 27 ++++ ArduPlane/radio.pde | 18 +-- libraries/APM_RC/APM_RC.h | 11 -- libraries/AP_Camera/AP_Camera.cpp | 113 -------------- libraries/AP_Camera/AP_Camera.h | 57 ------- libraries/AP_DCM/AP_DCM.h | 2 +- libraries/AP_Mount/AP_Mount.cpp | 192 ------------------------ libraries/AP_Mount/AP_Mount.h | 90 ----------- libraries/RC_Channel/RC_Channel.cpp | 2 +- libraries/RC_Channel/RC_Channel.h | 10 +- libraries/RC_Channel/RC_Channel_aux.cpp | 36 ----- libraries/RC_Channel/RC_Channel_aux.h | 8 - 18 files changed, 84 insertions(+), 650 deletions(-) delete mode 100644 ArduPlane/WP_activity.pde delete mode 100644 libraries/AP_Camera/AP_Camera.cpp delete mode 100644 libraries/AP_Camera/AP_Camera.h delete mode 100644 libraries/AP_Mount/AP_Mount.cpp delete mode 100644 libraries/AP_Mount/AP_Mount.h diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 4ebb342fd1..3a8bbb504f 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,4 +1,5 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // their default values, place the appropriate #define statements here. @@ -21,15 +22,4 @@ #define HIL_PORT 0 #define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #define GCS_PORT 3 -*/ - - -// ----- Camera definitions ------ -// ------------------------------ -#define CAMERA ENABLED -#define CAM_DEBUG DISABLED - -// - Options to reduce code size - -// ------------------------------- -// Disable text based terminal configuration -#define CLI_ENABLED DISABLED +*/ diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 4c560ef062..52b59655a0 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -304,6 +304,19 @@ //#define FLIGHT_MODE_6 MANUAL // +////////////////////////////////////////////////////////////////////////////// +// RC_5_FUNCT OPTIONAL +// RC_6_FUNCT OPTIONAL +// RC_7_FUNCT OPTIONAL +// RC_8_FUNCT OPTIONAL +// +// The channel 5 through 8 function assignments allow configuration of those +// channels for use with differential ailerons, flaps, flaperons, or camera +// or intrument mounts +// +//#define RC_5_FUNCT RC_5_FUNCT_NONE +//etc. + ////////////////////////////////////////////////////////////////////////////// // For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed // then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index a01fd6501f..9a0da7d2e8 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.24" +#define THISFIRMWARE "ArduPlane V2.24" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -41,9 +41,6 @@ version 2.1 of the License, or (at your option) any later version. #include // RC Channel Library #include // Range finder library #include -#include // Photo or video camera -#include // Camera mount - #include // MAVLink GCS definitions #include @@ -418,15 +415,6 @@ static unsigned long nav_loopTimer; // used to track the elapsed time for GP static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static float load; // % MCU cycles used -RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function - -//Camera tracking and stabilisation stuff -// -------------------------------------- -#if CAMERA == ENABLED -AP_Mount camera_mount(g_gps, &dcm); - -//pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) -#endif //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -575,18 +563,6 @@ static void fast_loop() static void medium_loop() { -#if CAMERA == ENABLED - // TODO replace home with a POI coming from a MavLink message or command - //camera_mount.set_GPS_target(home); - - // For now point the camera manually via the RC inputs (later remove these two lines) - // for simple dcm tests, replace k_manual with k_stabilise - camera_mount.set_mode(AP_Mount::k_stabilise); - camera_mount.update_mount(); - - g.camera.trigger_pic_cleanup(); -#endif - // This is the start of the medium (10 Hz) loop pieces // ----------------------------------------- switch(medium_loopCounter) { @@ -734,11 +710,6 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); - update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); - -#if CAMERA == ENABLED - camera_mount.update_mount_type(); -#endif break; case 2: diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7b62d9a8fa..570c38800a 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 12; + static const uint16_t k_format_version = 11; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -112,13 +112,6 @@ public: k_param_RTL_altitude, k_param_inverted_flight_ch, - // - // Camera parameters - // -#if CAMERA == ENABLED - k_param_camera, -#endif - // // 170: Radio settings // @@ -141,6 +134,11 @@ public: k_param_long_fs_action, k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, + + k_param_rc_5_funct, + k_param_rc_6_funct, + k_param_rc_7_funct, + k_param_rc_8_funct, // // 200: Feed-forward gains @@ -324,20 +322,19 @@ public: AP_Int8 flap_2_percent; AP_Int8 flap_2_speed; - // Camera -#if CAMERA == ENABLED - AP_Camera camera; -#endif - // RC channels RC_Channel channel_roll; RC_Channel channel_pitch; RC_Channel channel_throttle; RC_Channel channel_rudder; - RC_Channel_aux rc_5; - RC_Channel_aux rc_6; - RC_Channel_aux rc_7; - RC_Channel_aux rc_8; + RC_Channel rc_5; + RC_Channel rc_6; + RC_Channel rc_7; + RC_Channel rc_8; + AP_Int8 rc_5_funct; + AP_Int8 rc_6_funct; + AP_Int8 rc_7_funct; + AP_Int8 rc_8_funct; // PID controllers // @@ -431,13 +428,13 @@ public: inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")), airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")), + rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")), + rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")), + rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")), + rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")), // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! -#if CAMERA == ENABLED - camera (k_param_camera, PSTR("CAM_")), -#endif - // RC channel group key name //---------------------------------------------------------------------- channel_roll (k_param_channel_roll, PSTR("RC1_")), diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde deleted file mode 100644 index 474644cb90..0000000000 --- a/ArduPlane/WP_activity.pde +++ /dev/null @@ -1,56 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CAMERA == ENABLED -void waypoint_check() -{ - if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you - g.camera.trigger_pic(); - } // DO SOMETHIMNG - } - if(g.waypoint_index == 20){ // When here do whats underneath - g.camera.trigger_pic(); - } -} - -void picture_time_check() -{ - if (g.camera.picture_time == 1){ - if (wp_distance < g.camera.wp_distance_min){ - g.camera.trigger_pic(); // or any camera activation command - } - } -} -#endif - -void egg_waypoint() -{ - if (g_rc_function[RC_Channel_aux::k_egg_drop]) - { - float temp = (float)(current_loc.alt - home.alt) * .01; - float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; - - if(g.waypoint_index == 3){ - if(wp_distance < egg_dist){ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 100; - } - }else{ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 0; - } - } -} - -#if CAMERA == ENABLED -void johann_check() // if you aren't Johann it doesn't really matter :D -{ - APM_RC.OutputCh(CH_7,1500 + (350)); - if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want - if(wp_distance < g.camera.wp_distance_min){ // Get as close as it can for you - g.camera.trigger_pic(); - } - } - if(g.waypoint_index == 20){ // When here do whats underneath - g.camera.trigger_pic(); - } -} -#endif diff --git a/ArduPlane/config.h b/ArduPlane/config.h index d48b4cb73d..e6261d3560 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -226,6 +226,19 @@ # define CH8_MAX 2000 #endif +////////////////////////////////////////////////////////////////////////////// +#ifndef RC_5_FUNCT +# define RC_5_FUNCT RC_5_FUNCT_NONE +#endif +#ifndef RC_6_FUNCT +# define RC_6_FUNCT RC_6_FUNCT_NONE +#endif +#ifndef RC_7_FUNCT +# define RC_7_FUNCT RC_7_FUNCT_NONE +#endif +#ifndef RC_8_FUNCT +# define RC_8_FUNCT RC_8_FUNCT_NONE +#endif #ifndef FLAP_1_PERCENT # define FLAP_1_PERCENT 0 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 5c5b01b10f..b48deed2da 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,12 +41,39 @@ #define GPS_PROTOCOL_MTK16 6 #define GPS_PROTOCOL_AUTO 7 +// Radio channels +// Note channels are from 0! +// +// XXX these should be CH_n defines from RC.h at some point. +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 #define CH_RUDDER CH_4 #define CH_YAW CH_4 +#define RC_5_FUNCT_NONE 0 +#define RC_5_FUNCT_AILERON 1 +#define RC_5_FUNCT_FLAP_AUTO 2 +#define RC_5_FUNCT_FLAPERON 3 + +#define RC_6_FUNCT_NONE 0 +#define RC_6_FUNCT_AILERON 1 +#define RC_6_FUNCT_FLAP_AUTO 2 +#define RC_6_FUNCT_FLAPERON 3 + +#define RC_7_FUNCT_NONE 0 + +#define RC_8_FUNCT_NONE 0 + // HIL enumerations #define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_MAVLINK 2 diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 5b26479ce6..6cacb73e86 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -28,22 +28,6 @@ static void init_rc_in() G_RC_AUX(k_flap_auto)->set_range(0,100); G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); G_RC_AUX(k_flaperon)->set_range(0,100); -#if CAMERA == ENABLED - G_RC_AUX(k_mount_yaw)->set_range( - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); - G_RC_AUX(k_mount_pitch)->set_range( - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); - G_RC_AUX(k_mount_roll)->set_range( - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); - G_RC_AUX(k_cam_trigger)->set_range( - g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); - G_RC_AUX(k_cam_open)->set_range(0,100); -#endif - G_RC_AUX(k_egg_drop)->set_range(0,100); } static void init_rc_out() @@ -56,7 +40,7 @@ static void init_rc_out() APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); APM_RC.Init(); // APM Radio initialization diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index bcac692abe..236f85593d 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,17 +5,6 @@ #define MIN_PULSEWIDTH 900 #define MAX_PULSEWIDTH 2100 -// Radio channels -// Note channels are from 0! -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - #include class APM_RC_Class diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp deleted file mode 100644 index ddfbc2d8a3..0000000000 --- a/libraries/AP_Camera/AP_Camera.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include -#include <../RC_Channel/RC_Channel_aux.h> - -extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -extern long wp_distance; -extern "C" { -void relay_on(); -void relay_off(); -} - -void -AP_Camera::servo_pic() // Servo operated camera -{ - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) - { - g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max; - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles - } -} - -void -AP_Camera::relay_pic() // basic relay activation -{ - relay_on(); - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles -} - -void -AP_Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ -// TODO find a way to do this without using the global parameter g -// g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic == 10){ - servo_pic(); // triggering method - thr_pic = 0; -// g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void -AP_Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ -// TODO find a way to do this without using the global parameter g -// g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method -// g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void -AP_Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // TODO: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles -} - -// single entry point to take pictures -void -AP_Camera::trigger_pic() -{ - switch (trigger_type) - { - case 0: - servo_pic(); // Servo operated camera - break; - case 1: - relay_pic(); // basic relay activation - break; - case 2: - throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - break; - case 3: - distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - break; - case 4: - NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - break; - } -} - -// de-activate the trigger after some delay, but without using a delay() function -void -AP_Camera::trigger_pic_cleanup() -{ - if (keep_cam_trigg_active_cycles) - { - keep_cam_trigg_active_cycles --; - } - else - { - switch (trigger_type) - { - case 0: - case 2: - case 3: - G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; - break; - case 1: - // TODO for some strange reason the function call bellow gives a linker error - //relay_off(); - PORTL &= ~B00000100; // hardcoded version of relay_off(). Replace with a proper function call later. - break; - case 4: - digitalWrite(camtrig, LOW); - break; - } - } -} diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h deleted file mode 100644 index e1c520131a..0000000000 --- a/libraries/AP_Camera/AP_Camera.h +++ /dev/null @@ -1,57 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file AP_Camera.h -/// @brief Photo or video camera manager, with EEPROM-backed storage of constants. - -#ifndef AP_CAMERA_H -#define AP_CAMERA_H - -#include - -/// @class Camera -/// @brief Object managing a Photo or video camera -class AP_Camera{ -protected: - AP_Var_group _group; // must be before all vars to keep ctor init order correct - -public: - /// Constructor - /// - /// @param key EEPROM storage key for the camera configuration parameters. - /// @param name Optional name for the group. - /// - AP_Camera(AP_Var::Key key, const prog_char_t *name) : - _group(key, name), - trigger_type(&_group, 0, 0, name ? PSTR("TRIGG_TYPE") : 0), - picture_time (0), // waypoint trigger variable - thr_pic (0), // timer variable for throttle_pic - camtrig (83), // PK6 chosen as it not near anything so safer for soldering - keep_cam_trigg_active_cycles (0), - wp_distance_min (10) - {} - - // single entry point to take pictures - void trigger_pic(); - - // de-activate the trigger after some delay, but without using a delay() function - void trigger_pic_cleanup(); - - int picture_time; // waypoint trigger variable - long wp_distance_min; // take picture if distance to WP is smaller than this - -private: - uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active - int thr_pic; // timer variable for throttle_pic - int camtrig; // PK6 chosen as it not near anything so safer for soldering - - AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor - - void servo_pic(); // Servo operated camera - void relay_pic(); // basic relay activation - void throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - void distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - void NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - -}; - -#endif /* AP_CAMERA_H */ diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 8e8af7cb36..9a10eb03bc 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef AP_DCM_h #define AP_DCM_h -// temporarily include all other classes here +// teporarily include all other classes here // since this naming is a bit off from the // convention and the AP_DCM should be the top // header file diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp deleted file mode 100644 index ef931eb58c..0000000000 --- a/libraries/AP_Mount/AP_Mount.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include -#include <../RC_Channel/RC_Channel_aux.h> - -extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function - -AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) -{ - _dcm=dcm; - _gps=gps; -} - -void AP_Mount::set_pitch_yaw(int pitchCh, int yawCh) -{ -} - -void AP_Mount::set_GPS_target(Location targetGPSLocation) -{ - _target_GPS_location=targetGPSLocation; - - //set mode - _mount_mode=k_gps_target; - - //update mount position - update_mount(); -} - -void AP_Mount::set_assisted(int roll, int pitch, int yaw) -{ - _assist_angles.x = roll; - _assist_angles.y = pitch; - _assist_angles.z = yaw; - - //set mode - _mount_mode=k_assisted; - - //update mount position - update_mount(); -} - -//sets the servo angles for FPV, note angles are * 100 -void AP_Mount::set_mount_free_roam(int roll, int pitch, int yaw) -{ - _roam_angles.x=roll; - _roam_angles.y=pitch; - _roam_angles.z=yaw; - - //set mode - _mount_mode=k_roam; - - //now update mount position - update_mount(); -} - -//sets the servo angles for landing, note angles are * 100 -void AP_Mount::set_mount_landing(int roll, int pitch, int yaw) -{ - _landing_angles.x=roll; - _landing_angles.y=pitch; - _landing_angles.z=yaw; - - //set mode - _mount_mode=k_landing; - - //now update mount position - update_mount(); -} - -void AP_Mount::set_none() -{ - //set mode - _mount_mode=k_none; - - //now update mount position - update_mount(); -} - -void AP_Mount::update_mount() -{ - Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs - Vector3f targ; //holds target vector, var is used as temp in calcs - - switch(_mount_mode) - { - case k_gps_target: - { - if(_gps->fix) - { - calc_GPS_target_vector(&_target_GPS_location); - } - m = _dcm->get_dcm_transposed(); - targ = m*_GPS_vector; - roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll - pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch - break; - } - case k_stabilise: - { - // TODO replace this simplified implementation with a proper one - roll_angle = -_dcm->roll_sensor; - pitch_angle = -_dcm->pitch_sensor; - yaw_angle = -_dcm->yaw_sensor; - break; - } - case k_roam: - { - roll_angle=100*_roam_angles.x; - pitch_angle=100*_roam_angles.y; - yaw_angle=100*_roam_angles.z; - break; - } - case k_assisted: - { - m = _dcm->get_dcm_transposed(); - //rotate vector - targ = m*_assist_vector; - roll_angle = degrees(atan2(targ.y,targ.z))*100; //roll - pitch_angle = degrees(atan2(-targ.x,targ.z))*100; //pitch - break; - } - case k_landing: - { - roll_angle=100*_roam_angles.x; - pitch_angle=100*_roam_angles.y; - yaw_angle=100*_roam_angles.z; - break; - } - case k_manual: // radio manual control - if (g_rc_function[RC_Channel_aux::k_mount_roll]) - roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max); - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) - pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max); - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) - yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max); - break; - case k_none: - default: - { - //do nothing - break; - } - } - - // write the results to the servos - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); - G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); - G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); -} - -void AP_Mount::set_mode(MountMode mode) -{ - _mount_mode=mode; -} - -void AP_Mount::calc_GPS_target_vector(struct Location *target) -{ - _GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195; - _GPS_vector.y = (target->lat-_gps->latitude)*.01113195; - _GPS_vector.z = (_gps->altitude-target->alt); -} - -void -AP_Mount::update_mount_type() -{ - // Auto-detect the mount gimbal type depending on the functions assigned to the servos - if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) - { - _mount_type = k_pan_tilt; - } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL)) - { - _mount_type = k_tilt_roll; - } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) - { - _mount_type = k_pan_tilt_roll; - } -} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h deleted file mode 100644 index e80c3e294a..0000000000 --- a/libraries/AP_Mount/AP_Mount.h +++ /dev/null @@ -1,90 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/************************************************************ -* AP_mount -- library to control a 2 or 3 axis mount. * -* * -* Author: Joe Holdsworth; * -* Ritchie Wilson; * -* Amilcar Lucas; * -* * -* Purpose: Move a 2 or 3 axis mount attached to vehicle, * -* Used for mount to track targets or stabilise * -* camera plus other modes. * -* * -* Usage: Use in main code to control mounts attached to * -* vehicle. * -* * -*Comments: All angles in degrees * 100, distances in meters* -* unless otherwise stated. * - ************************************************************/ -#ifndef AP_Mount_H -#define AP_Mount_H - -//#include -#include -#include -#include - -class AP_Mount -{ -public: - //Constructors - AP_Mount(GPS *gps, AP_DCM *dcm); - - //enums - enum MountMode{ - k_gps_target = 0, - k_stabilise = 1, //note the correct English spelling :) - k_roam = 2, - k_assisted = 3, - k_landing = 4, - k_none = 5, - k_manual = 6 - }; - - enum MountType{ - k_pan_tilt = 0, //yaw-pitch - k_tilt_roll = 1, //pitch-roll - k_pan_tilt_roll = 2, //yaw-pitch-roll - }; - - //Accessors - void set_pitch_yaw(int pitchCh, int yawCh); - void set_pitch_roll(int pitchCh, int rollCh); - void set_pitch_roll_yaw(int pitchCh, int rollCh, int yawCh); - - void set_GPS_target(Location targetGPSLocation); //used to tell the mount to track GPS location - void set_assisted(int roll, int pitch, int yaw); - void set_mount_free_roam(int roll, int pitch, int yaw);//used in the FPV for example, - void set_mount_landing(int roll, int pitch, int yaw); //set mount landing position - void set_none(); - - //methods - void update_mount(); - void update_mount_type(); //Auto-detect the mount gimbal type depending on the functions assigned to the servos - void set_mode(MountMode mode); - - int pitch_angle; //degrees*100 - int roll_angle; //degrees*100 - int yaw_angle; //degrees*100 -protected: - //methods - void calc_GPS_target_vector(struct Location *target); - //void CalculateDCM(int roll, int pitch, int yaw); - //members - AP_DCM *_dcm; - GPS *_gps; - - MountMode _mount_mode; - MountType _mount_type; - - struct Location _target_GPS_location; - Vector3f _GPS_vector; //target vector calculated stored in meters - - Vector3i _roam_angles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _landing_angles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - - Vector3i _assist_angles; //used to keep angles that user has supplied from assisted targeting - Vector3f _assist_vector; //used to keep vector calculated from _AssistAngles -}; -#endif \ No newline at end of file diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index aa189dff15..24fae0f5ea 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -120,7 +120,7 @@ RC_Channel::calc_pwm(void) { if(_type == RC_CHANNEL_RANGE){ pwm_out = range_to_pwm(); - radio_out = (_reverse >=0 ? pwm_out + radio_min : radio_max - pwm_out); + radio_out = pwm_out + radio_min; }else if(_type == RC_CHANNEL_ANGLE_RAW){ pwm_out = (float)servo_out * .1; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 132e9146f3..aec2ebc998 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -7,11 +7,12 @@ #define RC_Channel_h #include +#include /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel{ - protected: + private: AP_Var_group _group; // must be before all vars to keep ctor init order correct public: @@ -102,7 +103,8 @@ class RC_Channel{ int16_t _low; }; -// This is ugly, but it fixes compilation on arduino -#include "RC_Channel_aux.h" - #endif + + + + diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 55ed6819af..18b35ea05e 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -5,42 +5,6 @@ extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -int16_t -RC_Channel_aux::closest_limit(int16_t angle) -{ - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - int16_t min = angle_min / 10; - int16_t max = angle_max / 10; - - // Make sure the angle lies in the interval [-180 .. 180[ degrees - while (angle < -1800) angle += 3600; - while (angle >= 1800) angle -= 3600; - - // Make sure the angle limits lie in the interval [-180 .. 180[ degrees - while (min < -1800) min += 3600; - while (min >= 1800) min -= 3600; - while (max < -1800) max += 3600; - while (max >= 1800) max -= 3600; - // This is done every time because the user might change the min, max values on the fly - set_range(min, max); - - // If the angle is outside servo limits, saturate the angle to the closest limit - // On a circle the closest angular position must be carefully calculated to account for wrap-around - if ((angle < min) && (angle > max)){ - // angle error if min limit is used - int16_t err_min = min - angle + (anglemax?0:3600); // add 360 degrees if on the "wrong side" - angle = err_min Date: Tue, 13 Sep 2011 01:51:35 +0200 Subject: [PATCH 72/74] Ignore some files that should never be commited --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..1d314f6281 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.metadata/ +Tools/ArdupilotMegaPlanner/bin/Release/logs/ +config.mk From f08cea8044c7075d04419698896006f5b4ecd414 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 01:54:47 +0200 Subject: [PATCH 73/74] These changes were meant to be inside commit a14c06adc06b. I'm sorry but reverting stuff is not that easy --- ArduPlane/APM_Config.h | 3 +-- ArduPlane/APM_Config.h.reference | 13 ------------- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/Parameters.h | 23 +++++------------------ ArduPlane/config.h | 13 ------------- ArduPlane/defines.h | 27 --------------------------- libraries/APM_RC/APM_RC.h | 11 +++++++++++ libraries/AP_DCM/AP_DCM.h | 2 +- libraries/RC_Channel/RC_Channel.cpp | 2 +- libraries/RC_Channel/RC_Channel.h | 10 ++++------ 10 files changed, 26 insertions(+), 81 deletions(-) diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 3a8bbb504f..40fcc180a9 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,5 +1,4 @@ - - // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // their default values, place the appropriate #define statements here. diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 52b59655a0..4c560ef062 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -304,19 +304,6 @@ //#define FLIGHT_MODE_6 MANUAL // -////////////////////////////////////////////////////////////////////////////// -// RC_5_FUNCT OPTIONAL -// RC_6_FUNCT OPTIONAL -// RC_7_FUNCT OPTIONAL -// RC_8_FUNCT OPTIONAL -// -// The channel 5 through 8 function assignments allow configuration of those -// channels for use with differential ailerons, flaps, flaperons, or camera -// or intrument mounts -// -//#define RC_5_FUNCT RC_5_FUNCT_NONE -//etc. - ////////////////////////////////////////////////////////////////////////////// // For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed // then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 9a0da7d2e8..1c71b6b5be 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -415,6 +415,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed time for GP static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static float load; // % MCU cycles used +RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -710,6 +711,8 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); + break; case 2: diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 570c38800a..70976ce887 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 11; + static const uint16_t k_format_version = 12; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -134,11 +134,6 @@ public: k_param_long_fs_action, k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, - - k_param_rc_5_funct, - k_param_rc_6_funct, - k_param_rc_7_funct, - k_param_rc_8_funct, // // 200: Feed-forward gains @@ -327,14 +322,10 @@ public: RC_Channel channel_pitch; RC_Channel channel_throttle; RC_Channel channel_rudder; - RC_Channel rc_5; - RC_Channel rc_6; - RC_Channel rc_7; - RC_Channel rc_8; - AP_Int8 rc_5_funct; - AP_Int8 rc_6_funct; - AP_Int8 rc_7_funct; - AP_Int8 rc_8_funct; + RC_Channel_aux rc_5; + RC_Channel_aux rc_6; + RC_Channel_aux rc_7; + RC_Channel_aux rc_8; // PID controllers // @@ -428,10 +419,6 @@ public: inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")), airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")), - rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")), - rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")), - rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")), - rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")), // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! diff --git a/ArduPlane/config.h b/ArduPlane/config.h index e6261d3560..d48b4cb73d 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -226,19 +226,6 @@ # define CH8_MAX 2000 #endif -////////////////////////////////////////////////////////////////////////////// -#ifndef RC_5_FUNCT -# define RC_5_FUNCT RC_5_FUNCT_NONE -#endif -#ifndef RC_6_FUNCT -# define RC_6_FUNCT RC_6_FUNCT_NONE -#endif -#ifndef RC_7_FUNCT -# define RC_7_FUNCT RC_7_FUNCT_NONE -#endif -#ifndef RC_8_FUNCT -# define RC_8_FUNCT RC_8_FUNCT_NONE -#endif #ifndef FLAP_1_PERCENT # define FLAP_1_PERCENT 0 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b48deed2da..5c5b01b10f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,39 +41,12 @@ #define GPS_PROTOCOL_MTK16 6 #define GPS_PROTOCOL_AUTO 7 -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 #define CH_RUDDER CH_4 #define CH_YAW CH_4 -#define RC_5_FUNCT_NONE 0 -#define RC_5_FUNCT_AILERON 1 -#define RC_5_FUNCT_FLAP_AUTO 2 -#define RC_5_FUNCT_FLAPERON 3 - -#define RC_6_FUNCT_NONE 0 -#define RC_6_FUNCT_AILERON 1 -#define RC_6_FUNCT_FLAP_AUTO 2 -#define RC_6_FUNCT_FLAPERON 3 - -#define RC_7_FUNCT_NONE 0 - -#define RC_8_FUNCT_NONE 0 - // HIL enumerations #define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_MAVLINK 2 diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 236f85593d..bcac692abe 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,6 +5,17 @@ #define MIN_PULSEWIDTH 900 #define MAX_PULSEWIDTH 2100 +// Radio channels +// Note channels are from 0! +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + #include class APM_RC_Class diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 9a10eb03bc..8e8af7cb36 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef AP_DCM_h #define AP_DCM_h -// teporarily include all other classes here +// temporarily include all other classes here // since this naming is a bit off from the // convention and the AP_DCM should be the top // header file diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 24fae0f5ea..aa189dff15 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -120,7 +120,7 @@ RC_Channel::calc_pwm(void) { if(_type == RC_CHANNEL_RANGE){ pwm_out = range_to_pwm(); - radio_out = pwm_out + radio_min; + radio_out = (_reverse >=0 ? pwm_out + radio_min : radio_max - pwm_out); }else if(_type == RC_CHANNEL_ANGLE_RAW){ pwm_out = (float)servo_out * .1; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index aec2ebc998..132e9146f3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -7,12 +7,11 @@ #define RC_Channel_h #include -#include /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel{ - private: + protected: AP_Var_group _group; // must be before all vars to keep ctor init order correct public: @@ -103,8 +102,7 @@ class RC_Channel{ int16_t _low; }; +// This is ugly, but it fixes compilation on arduino +#include "RC_Channel_aux.h" + #endif - - - - From 1f291977718d45293e94676c30e7aea542334275 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 14:01:35 +0200 Subject: [PATCH 74/74] Revert more of 6dcbc7f44bc0. Fix an issue with initialization of channels that on-line changed their function. For the curious people that the code size overhead of having any aux channel do any function (with this code) is 44 bytes. To see wich code I'm talking about, do a git diff 05057ac2d455..this_commit (replace this commit with the commit hash of this commit) --- ArduPlane/radio.pde | 4 ---- libraries/RC_Channel/RC_Channel_aux.cpp | 5 +++++ libraries/RC_Channel/RC_Channel_aux.h | 7 +------ 3 files changed, 6 insertions(+), 10 deletions(-) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 6cacb73e86..674c096e25 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -24,10 +24,6 @@ static void init_rc_in() //set auxiliary ranges update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); - G_RC_AUX(k_flap)->set_range(0,100); - G_RC_AUX(k_flap_auto)->set_range(0,100); - G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); - G_RC_AUX(k_flaperon)->set_range(0,100); } static void init_rc_out() diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 18b35ea05e..1ff7b45326 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -47,4 +47,9 @@ void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Ch g_rc_function[aux_servo_function[CH_6]] = rc_6; g_rc_function[aux_servo_function[CH_7]] = rc_7; g_rc_function[aux_servo_function[CH_8]] = rc_8; + + G_RC_AUX(k_flap)->set_range(0,100); + G_RC_AUX(k_flap_auto)->set_range(0,100); + G_RC_AUX(k_aileron)->set_angle(4500); + G_RC_AUX(k_flaperon)->set_range(0,100); } diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 556ffa6c3b..7e091bf18f 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -14,7 +14,6 @@ /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function -/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements class RC_Channel_aux : public RC_Channel{ public: /// Constructor @@ -24,9 +23,7 @@ public: /// RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) : RC_Channel(key, name), - function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name - angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection - angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection + function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0) // suppress name if group has no name {} typedef enum @@ -41,8 +38,6 @@ public: } Aux_servo_function_t; AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop - AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units - AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it