From a1b2cc7229d43e0c9a2bffc86ce768f6e55304bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:45:13 +1000 Subject: [PATCH 001/100] 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 1ca13fcd3bfc5943d1a3fad968d1867917a767e0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:49:11 +1000 Subject: [PATCH 002/100] 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 6e2f231688586d5fa1fc790bdc301c6426fdb84a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:51:21 +1000 Subject: [PATCH 003/100] 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 581603c3b722ddf27dd9216c2c56680725306816 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Fri, 9 Sep 2011 16:02:22 +0200 Subject: [PATCH 004/100] 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 9e80f2e9202a1ae803ce7858bbd469542984cf07 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Fri, 9 Sep 2011 16:18:38 +0200 Subject: [PATCH 005/100] 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 b977007bdff1898c6450b6e366908587cd1bde8a Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 13:26:29 +0200 Subject: [PATCH 006/100] 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 ada409855ad5c950eadfea3b977a6d284bb40f45 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 13:47:09 +0200 Subject: [PATCH 007/100] 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 bf7d62e788f02f8cc1f5b83e07c2bd9930a114a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:06:46 +1000 Subject: [PATCH 008/100] 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 a2d602b090a83b0f0a0b184c644bb66a3af6dad0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:08:18 +1000 Subject: [PATCH 009/100] 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 6d059b8eba1161bfeda1fd9cc0241afe434edef6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 23:24:50 +1000 Subject: [PATCH 010/100] 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 329df0415728de4a7b4c8ab6ec5326ec034ae5e6 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:35:23 +0200 Subject: [PATCH 011/100] 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 f60969a5d5b46be482a498b4884926d08a763795 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:36:47 +0200 Subject: [PATCH 012/100] 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 93e13ace970f10e0a319dafffa4c4393421603c7 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:57:27 +0200 Subject: [PATCH 013/100] 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 5fc771f9d8a643952237d9f171179f57ac97bc12 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:58:25 +0200 Subject: [PATCH 014/100] 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 a78de771d4a3701e6352fde9a43f15aa21bd7d33 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:34:47 +0200 Subject: [PATCH 015/100] 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 5bf21d5159a5721d3981af3750b111b58a805a66 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:36:39 +0200 Subject: [PATCH 016/100] 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 5454cd503b766022f1751b990796c702d14c0065 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:40:13 +0200 Subject: [PATCH 017/100] 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 a0dc3b552e8e1239b8b54715c3a42810cdb8c995 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:41:48 +0200 Subject: [PATCH 018/100] 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 f53364bf4180c759412a724b396a0078f0464f49 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 04:04:02 +0200 Subject: [PATCH 019/100] 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 020/100] 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 021/100] 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 59bd4d731a0e459e8403c8853adb0fb062c7e8bf Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 04:12:46 +0200 Subject: [PATCH 022/100] 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 ed112b224384ea426a05bbb8dfd0023c72d27553 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 17:41:18 +0200 Subject: [PATCH 023/100] 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 7544b9abd31407c79124bc87ffc24a66a6c5dd92 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 18:32:24 +0200 Subject: [PATCH 024/100] 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 966f9f962ad366dd13ee36e025df319b415fd65c Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 19:13:01 +0200 Subject: [PATCH 025/100] 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 23:07:30 +0200 Subject: [PATCH 026/100] 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 69a9f44fee6f999e25c5b12acb492e6df6088c95 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 23:25:06 +0200 Subject: [PATCH 027/100] 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 6dfa53bcb138072cf576995bf5e505e4d29abb31 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sun, 11 Sep 2011 23:32:04 +0200 Subject: [PATCH 028/100] 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 b8832fe9ab2c6ccc900b2eadfc8559f6d77d6581 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 00:02:47 +0200 Subject: [PATCH 029/100] 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 36c982d84ce3f088a33a890b5ba87f244be14887 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Mon, 12 Sep 2011 00:43:54 +0200 Subject: [PATCH 030/100] 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 f2e33f830345ae05b87a7e66f4a9a5f74aa82745 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:27:56 -0700 Subject: [PATCH 031/100] 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 689bfb803ceb6e3b9e6f57f13777a11112999461 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:28:43 -0700 Subject: [PATCH 032/100] 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 1ae63e2a4ce31aa217266d212a6c11d517e2dedc Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:32:58 -0700 Subject: [PATCH 033/100] 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 efeb1555ba8160f0e15e7207a92b09e1e55f66f1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:33:29 -0700 Subject: [PATCH 034/100] 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 7d57dfa3ecd9338dca429b73399c5b61011389c2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:36:20 -0700 Subject: [PATCH 035/100] 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 7e7961212511da7dc48b112369e771a5c32cf041 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:40:05 -0700 Subject: [PATCH 036/100] 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 1a8b74063a85ba2a99b2805698ef18a2ce22c8ae Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 21:50:32 -0600 Subject: [PATCH 037/100] 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 d6370fd38933ee5c64a1efa2b14274ea90ded9f1 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 21:59:46 -0600 Subject: [PATCH 038/100] 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 ae77029c9892dc9c4686b0183ad816caa926406d Mon Sep 17 00:00:00 2001 From: Mike Smith Date: Sun, 11 Sep 2011 22:20:54 -0700 Subject: [PATCH 039/100] 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 2a0912b867c19456b65bf4a962f3cb49c6258cdd Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Mon, 12 Sep 2011 07:24:52 -0600 Subject: [PATCH 040/100] 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 5a14d546bb003bf765ac45058f9fe602413e1534 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 17:37:42 -0600 Subject: [PATCH 041/100] 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 f7f745055dc3de38b6ec24da9c62f4c43cb75b90 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 20:03:44 +0200 Subject: [PATCH 042/100] 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 e84a422bd4f31f3861345e19cc15f52ab2ea7046 Mon Sep 17 00:00:00 2001 From: DrZiplok Date: Mon, 12 Sep 2011 18:43:31 +0000 Subject: [PATCH 043/100] 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 e78d6dc046cd63c7ff779b2469855ddefd137b49 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 21:24:54 +0200 Subject: [PATCH 044/100] 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 49b31299efd39d4ac9af07ecb986d078c833c016 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 21:32:05 +0200 Subject: [PATCH 045/100] 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 4a2f58b013999aa71364fd215eba3abddda67560 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 12 Sep 2011 12:57:36 -0700 Subject: [PATCH 046/100] 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 dd843d18ec639a750f050a9eff303be359a4e0f7 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 01:24:06 +0200 Subject: [PATCH 047/100] 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 048/100] 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 ef955eace4b55d951b3fb79076d4a78797a1b044 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 01:54:47 +0200 Subject: [PATCH 049/100] 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 c58b563584cdc9a417a823fd0e9e0eb9593508e1 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 13 Sep 2011 14:01:35 +0200 Subject: [PATCH 050/100] 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 From a6ca3d23b5f2b804329da3e517980b30b0eaa8b1 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Wed, 14 Sep 2011 21:31:00 +0800 Subject: [PATCH 051/100] APM Planner 1.0.68 bug fix test srtm --- .../ArdupilotMegaPlanner/ArdupilotMega.csproj | 5 +- Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs | 1 - .../GCSViews/Configuration.cs | 16 ++-- .../ArdupilotMegaPlanner/GCSViews/Firmware.cs | 7 +- .../GCSViews/Firmware.resx | 12 +-- .../GCSViews/FlightPlanner.cs | 2 +- Tools/ArdupilotMegaPlanner/HUD.cs | 14 +++- Tools/ArdupilotMegaPlanner/Log.cs | 39 ---------- Tools/ArdupilotMegaPlanner/MAVLink.cs | 15 +++- Tools/ArdupilotMegaPlanner/MainV2.cs | 2 + Tools/ArdupilotMegaPlanner/MyButton.cs | 2 +- .../Properties/AssemblyInfo.cs | 2 +- Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 1 - .../bin/Release/GCSViews/Firmware.resx | 12 +-- Tools/ArdupilotMegaPlanner/srtm.cs | 76 +++++++++++++++++++ 15 files changed, 137 insertions(+), 69 deletions(-) create mode 100644 Tools/ArdupilotMegaPlanner/srtm.cs diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 6ac38ed914..ddfdd3e8d6 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -232,7 +232,6 @@ UserControl - Component @@ -302,7 +301,6 @@ ElevationProfile.cs - Component @@ -338,6 +336,7 @@ Splash.cs + Form @@ -374,6 +373,7 @@ Firmware.cs + Designer FlightData.cs @@ -424,6 +424,7 @@ Firmware.cs Always + Designer FlightData.cs diff --git a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs index 99660f265f..94e3281f84 100644 --- a/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs +++ b/Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs @@ -6,7 +6,6 @@ using System.IO.Ports; using System.Threading; using System.Net; // dns, ip address using System.Net.Sockets; // tcplistner -using SerialProxy; namespace System.IO.Ports { diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs index 9dba66f3d1..8758775aac 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs @@ -257,6 +257,8 @@ namespace ArdupilotMega.GCSViews internal void processToScreen() { + + Params.Rows.Clear(); // process hashdefines and update display @@ -541,7 +543,10 @@ namespace ArdupilotMega.GCSViews private void BUT_writePIDS_Click(object sender, EventArgs e) { - foreach (string value in changes.Keys) + + Hashtable temp = (Hashtable)changes.Clone(); + + foreach (string value in temp.Keys) { try { @@ -566,6 +571,7 @@ namespace ArdupilotMega.GCSViews if (row.Cells[0].Value.ToString() == value) { row.Cells[1].Style.BackColor = Color.FromArgb(0x43, 0x44, 0x45); + changes.Remove(value); break; } } @@ -575,8 +581,6 @@ namespace ArdupilotMega.GCSViews } catch { MessageBox.Show("Set " + value + " Failed"); } } - - changes.Clear(); } const float rad2deg = (float)(180 / Math.PI); @@ -600,9 +604,11 @@ namespace ArdupilotMega.GCSViews MainV2.fixtheme(temp); - TabSetup.Controls.Clear(); + temp.ShowDialog(); - TabSetup.Controls.Add(temp.Controls[0]); + startup = true; + processToScreen(); + startup = false; } } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index 1e7c2544c7..15f3a849f7 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -519,13 +519,18 @@ namespace ArdupilotMega.GCSViews } catch (Exception ex) { lbl_status.Text = "Failed download"; MessageBox.Show("Failed to download new firmware : " + ex.Message); return; } + UploadFlash(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex", board); + } + + void UploadFlash(string filename, string board) + { byte[] FLASH = new byte[1]; StreamReader sr = null; try { lbl_status.Text = "Reading Hex File"; this.Refresh(); - sr = new StreamReader(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex"); + sr = new StreamReader(filename); FLASH = readIntelHEXv2(sr); sr.Close(); Console.WriteLine("\n\nSize: {0}\n\n", FLASH.Length); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx index 95f999f7a7..7283c0a474 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.resx @@ -343,16 +343,16 @@ NoControl - 101, 165 + 113, 167 - 79, 13 + 56, 13 8 - ArduPilot Mega + ArduPlane label1 @@ -403,16 +403,16 @@ NoControl - 57, 362 + 74, 361 - 168, 13 + 142, 13 10 - ArduPilot Mega (Xplane simulator) + ArduPlane (Xplane simulator) label3 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 21732c373e..e144fa180a 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -347,7 +347,7 @@ namespace ArdupilotMega.GCSViews { TXT_mouselat.Text = lat.ToString(); TXT_mouselong.Text = lng.ToString(); - TXT_mousealt.Text = alt.ToString(); + TXT_mousealt.Text = srtm.getAltitude(lat, lng).ToString("0"); try { diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs index b4e8e178f4..b95cd51577 100644 --- a/Tools/ArdupilotMegaPlanner/HUD.cs +++ b/Tools/ArdupilotMegaPlanner/HUD.cs @@ -236,7 +236,7 @@ System.ComponentModel.Category("Values")] { //Console.WriteLine("ms "+(DateTime.Now - starttime).TotalMilliseconds); //e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); - return; + //return; } starttime = DateTime.Now; @@ -1365,6 +1365,18 @@ System.ComponentModel.Category("Values")] charbitmaps = new Bitmap[charbitmaps.Length]; + try + { + + foreach (int texid in charbitmaptexid) + { + if (texid != 0) + GL.DeleteTexture(texid); + } + + } + catch { } + GC.Collect(); try diff --git a/Tools/ArdupilotMegaPlanner/Log.cs b/Tools/ArdupilotMegaPlanner/Log.cs index e47cb2c35d..e51d159de0 100644 --- a/Tools/ArdupilotMegaPlanner/Log.cs +++ b/Tools/ArdupilotMegaPlanner/Log.cs @@ -366,45 +366,6 @@ namespace ArdupilotMega } } - private byte[] readline(NetSerial comport) - { - byte[] temp = new byte[1024]; - int count = 0; - int timeout = 0; - - while (timeout <= 100) - { - if (!comport.IsOpen) { break; } - if (comport.BytesToRead > 0) - { - byte letter = (byte)comport.ReadByte(); - - temp[count] = letter; - - if (letter == '\n') // normal line - { - break; - } - - - count++; - if (count == temp.Length) - break; - timeout = 0; - } else { - timeout++; - System.Threading.Thread.Sleep(5); - } - } - if (timeout >= 100) { - Console.WriteLine("readline timeout"); - } - - Array.Resize(ref temp, count + 1); - - return temp; - } - List modelist = new List(); List[] position = new List[200]; int positionindex = 0; diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 0c26ebb6cd..242b2818aa 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -951,6 +951,8 @@ namespace ArdupilotMega req.seq = index; + //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); + // request generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST, req); @@ -972,11 +974,14 @@ namespace ArdupilotMega MainV2.givecomport = false; throw new Exception("Timeout on read - getWP"); } + //Console.WriteLine("getwp read " + DateTime.Now.Millisecond); byte[] buffer = readPacket(); + //Console.WriteLine("getwp readend " + DateTime.Now.Millisecond); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT) { + //Console.WriteLine("getwp ans " + DateTime.Now.Millisecond); __mavlink_waypoint_t wp = new __mavlink_waypoint_t(); object temp = (object)wp; @@ -1070,7 +1075,7 @@ namespace ArdupilotMega } else { - //Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); + Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]); } } } @@ -1455,16 +1460,18 @@ namespace ArdupilotMega else { int to = 0; - while (BaseStream.BytesToRead < length) + while (BaseStream.BytesToRead < (length - 4)) { if (to > 1000) { Console.WriteLine("MAVLINK: wait time out btr {0} len {1}", BaseStream.BytesToRead, length); break; } - System.Threading.Thread.Sleep(2); - System.Windows.Forms.Application.DoEvents(); + System.Threading.Thread.Sleep(1); + //System.Windows.Forms.Application.DoEvents(); to++; + + //Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead); } int read = BaseStream.Read(temp, 6, length - 4); } diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 48dac31e32..451e8acea8 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -76,6 +76,8 @@ namespace ArdupilotMega //System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US"); //System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US"); + srtm.datadirectory = @"C:\srtm"; + var t = Type.GetType("Mono.Runtime"); MAC = (t != null); diff --git a/Tools/ArdupilotMegaPlanner/MyButton.cs b/Tools/ArdupilotMegaPlanner/MyButton.cs index 12508be1f7..11bf6cd531 100644 --- a/Tools/ArdupilotMegaPlanner/MyButton.cs +++ b/Tools/ArdupilotMegaPlanner/MyButton.cs @@ -50,7 +50,7 @@ namespace ArdupilotMega if (!this.Enabled) { - SolidBrush brush = new SolidBrush(Color.FromArgb(200, 0x2b, 0x3a, 0x03)); + SolidBrush brush = new SolidBrush(Color.FromArgb(150, 0x2b, 0x3a, 0x03)); gr.FillRectangle(brush, 0, 0, this.Width, this.Height); } diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 25abe98b99..d4f74b5646 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.67")] +[assembly: AssemblyFileVersion("1.0.68")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 53279ab634..17c296b914 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -231,7 +231,6 @@ namespace ArdupilotMega.Setup MainV2.comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RC_CHANNELS, oldrc); - MainV2.comPort.param = MainV2.comPort.getParamList(); if (Configuration != null) { Configuration.startup = true; diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx index 95f999f7a7..7283c0a474 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Firmware.resx @@ -343,16 +343,16 @@ NoControl - 101, 165 + 113, 167 - 79, 13 + 56, 13 8 - ArduPilot Mega + ArduPlane label1 @@ -403,16 +403,16 @@ NoControl - 57, 362 + 74, 361 - 168, 13 + 142, 13 10 - ArduPilot Mega (Xplane simulator) + ArduPlane (Xplane simulator) label3 diff --git a/Tools/ArdupilotMegaPlanner/srtm.cs b/Tools/ArdupilotMegaPlanner/srtm.cs new file mode 100644 index 0000000000..e11c6ecb9b --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/srtm.cs @@ -0,0 +1,76 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO; + +namespace ArdupilotMega +{ + class srtm + { + public static string datadirectory; + + public static int getAltitude(double lat, double lng) + { + short alt = -32768; + + lat += 0.0008; + //lng += 0.0008; + + int x = (int)Math.Floor(lng); + int y = (int)Math.Floor(lat); + + string ns; + if (y > 0) + ns = "N"; + else + ns = "S"; + + string ew; + if (x > 0) + ew = "E"; + else + ew = "W"; + + string filename = ns+ Math.Abs(y).ToString("00")+ew+ Math.Abs(x).ToString("000")+".hgt"; + + if (!File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) + { + return alt; + } + + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open,FileAccess.Read); + + float posx = 0; + float row = 0; + + if (fs.Length <= (1201 * 1201 * 2)) { + posx = (int)(((float)(lng - x)) * (1201 * 2)); + row = (int)(((float)(lat - y)) * 1201) * (1201 * 2); + row = (1201 * 1201 * 2) - row; + } else { + posx = (int)(((float)(lng - x)) * (3601 * 2)); + row = (int)(((float)(lat - y)) * 3601) * (3601 * 2); + row = (3601 * 3601 * 2) - row; + } + + if (posx % 2 == 1) + { + posx--; + } + + //Console.WriteLine(filename + " row " + row + " posx" + posx); + + byte[] data = new byte[2]; + + fs.Seek((int)(row + posx), SeekOrigin.Begin); + fs.Read(data, 0, data.Length); + + Array.Reverse(data); + + alt = BitConverter.ToInt16(data,0); + + return alt; + } + } +} From 9f028bb5a038c4c9862cd5a103face16fee6f0ed Mon Sep 17 00:00:00 2001 From: unknown Date: Wed, 14 Sep 2011 15:44:55 +0200 Subject: [PATCH 052/100] Adding CMake support --- ArduCopter/CMakeLists.txt | 162 ++++++ ArduPlane/CMakeLists.txt | 162 ++++++ cmake/PdeP.jar | Bin 0 -> 5690 bytes cmake/modules/ArduinoProcessing.cmake | 103 ++++ cmake/modules/FindArduino.cmake | 581 ++++++++++++++++++++++ cmake/modules/Platform/Arduino.cmake | 0 cmake/modules/Platform/ArduinoPaths.cmake | 21 + cmake/toolchains/Arduino.cmake | 66 +++ libraries/APM_BMP085/CMakeLists.txt | 22 + libraries/APM_PI/CMakeLists.txt | 24 + libraries/APM_RC/CMakeLists.txt | 22 + libraries/AP_ADC/CMakeLists.txt | 26 + libraries/AP_Common/CMakeLists.txt | 33 ++ libraries/AP_Compass/CMakeLists.txt | 27 + libraries/AP_DCM/CMakeLists.txt | 25 + libraries/AP_GPS/CMakeLists.txt | 44 ++ libraries/AP_IMU/CMakeLists.txt | 25 + libraries/AP_Math/CMakeLists.txt | 24 + libraries/AP_OpticalFlow/CMakeLists.txt | 24 + libraries/AP_PID/CMakeLists.txt | 24 + libraries/AP_RangeFinder/CMakeLists.txt | 27 + libraries/CMakeLists.txt | 24 + libraries/DataFlash/CMakeLists.txt | 21 + libraries/FastSerial/CMakeLists.txt | 28 ++ libraries/GCS_MAVLink/CMakeLists.txt | 19 + libraries/ModeFilter/CMakeLists.txt | 24 + libraries/PID/CMakeLists.txt | 21 + libraries/RC_Channel/CMakeLists.txt | 24 + libraries/memcheck/CMakeLists.txt | 24 + 29 files changed, 1627 insertions(+) create mode 100644 ArduCopter/CMakeLists.txt create mode 100644 ArduPlane/CMakeLists.txt create mode 100644 cmake/PdeP.jar create mode 100644 cmake/modules/ArduinoProcessing.cmake create mode 100644 cmake/modules/FindArduino.cmake create mode 100644 cmake/modules/Platform/Arduino.cmake create mode 100644 cmake/modules/Platform/ArduinoPaths.cmake create mode 100644 cmake/toolchains/Arduino.cmake create mode 100644 libraries/APM_BMP085/CMakeLists.txt create mode 100644 libraries/APM_PI/CMakeLists.txt create mode 100644 libraries/APM_RC/CMakeLists.txt create mode 100644 libraries/AP_ADC/CMakeLists.txt create mode 100644 libraries/AP_Common/CMakeLists.txt create mode 100644 libraries/AP_Compass/CMakeLists.txt create mode 100644 libraries/AP_DCM/CMakeLists.txt create mode 100644 libraries/AP_GPS/CMakeLists.txt create mode 100644 libraries/AP_IMU/CMakeLists.txt create mode 100644 libraries/AP_Math/CMakeLists.txt create mode 100644 libraries/AP_OpticalFlow/CMakeLists.txt create mode 100644 libraries/AP_PID/CMakeLists.txt create mode 100644 libraries/AP_RangeFinder/CMakeLists.txt create mode 100644 libraries/CMakeLists.txt create mode 100644 libraries/DataFlash/CMakeLists.txt create mode 100644 libraries/FastSerial/CMakeLists.txt create mode 100644 libraries/GCS_MAVLink/CMakeLists.txt create mode 100644 libraries/ModeFilter/CMakeLists.txt create mode 100644 libraries/PID/CMakeLists.txt create mode 100644 libraries/RC_Channel/CMakeLists.txt create mode 100644 libraries/memcheck/CMakeLists.txt diff --git a/ArduCopter/CMakeLists.txt b/ArduCopter/CMakeLists.txt new file mode 100644 index 0000000000..bef981f926 --- /dev/null +++ b/ArduCopter/CMakeLists.txt @@ -0,0 +1,162 @@ +#=============================================================================# +# Author: Niklaa Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") + +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +#include(ArduinoProcessing) + +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + + +message(STATUS "DIR: ${CMAKE_SOURCE_DIR}") + +cmake_minimum_required(VERSION 2.8) +#====================================================================# +# Setup Project # +#====================================================================# +project(ArduCopter C CXX) + +find_package(Arduino 22 REQUIRED) + +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter) +#add_subdirectory(testtool) + +PRINT_BOARD_SETTINGS(mega2560) + + + +#=============================================================================# +# Author: Niklas Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME arducopter) + +set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ArduCopter.pde + Attitude.pde + Camera.pde + commands.pde + commands_logic.pde + commands_process.pde + control_modes.pde + events.pde + flight_control.pde + flip.pde + GCS.pde + GCS_Ardupilot.pde + #GCS_IMU_output.pde + GCS_Jason_text.pde + GCS_Mavlink.pde + GCS_Standard.pde + GCS_Xplane.pde + heli.pde + HIL_Xplane.pde + leds.pde + Log.pde + motors_hexa.pde + motors_octa.pde + motors_octa_quad.pde + motors_quad.pde + motors_tri.pde + motors_y6.pde + navigation.pde + planner.pde + radio.pde + read_commands.pde + sensors.pde + setup.pde + system.pde + test.pde + ) # Firmware sketches + +#create dummy sourcefile +file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit") + +set(${FIRMWARE_NAME}_SRCS + #test.cpp + ${FIRMWARE_NAME}.cpp + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + APM_Config.h + APM_Config_mavlink_hil.h + APM_Config_xplane.h + config.h + defines.h + GCS.h + HIL.h + Mavlink_Common.h + Parameters.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + DataFlash + AP_Math + PID + RC_Channel + AP_OpticalFlow + ModeFilter + memcheck + #AP_PID + APM_PI + #APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + c + m +) +SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/DataFlash +${CMAKE_SOURCE_DIR}/libraries/AP_Math +${CMAKE_SOURCE_DIR}/libraries/PID +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/RC_Channel +${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/memcheck +#${CMAKE_SOURCE_DIR}/libraries/AP_PID +${CMAKE_SOURCE_DIR}/libraries/APM_PI +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt new file mode 100644 index 0000000000..1509de51a6 --- /dev/null +++ b/ArduPlane/CMakeLists.txt @@ -0,0 +1,162 @@ +#=============================================================================# +# Author: Niklaa Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") + +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +#include(ArduinoProcessing) + +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + +cmake_minimum_required(VERSION 2.8) +#====================================================================# +# Setup Project # +#====================================================================# +project(ArduPlane C CXX) + +find_package(Arduino 22 REQUIRED) + +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter) +#add_subdirectory(testtool) + +PRINT_BOARD_SETTINGS(mega2560) + + + +#=============================================================================# +# Author: Niklas Goddemeier & Sebastian Rohde # +# Date: 04.09.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ArduPlane) + +set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ArduPlane.pde + Attitude.pde + climb_rate.pde + commands.pde + commands_logic.pde + commands_process.pde + control_modes.pde + events.pde + #flight_control.pde + #flip.pde + #GCS.pde + #GCS_Ardupilot.pde + #GCS_IMU_output.pde + #GCS_Jason_text.pde + GCS_Mavlink.pde + #GCS_Standard.pde + #GCS_Xplane.pde + #heli.pde + HIL_Xplane.pde + #leds.pde + Log.pde + #motors_hexa.pde + #motors_octa.pde + #motors_octa_quad.pde + #motors_quad.pde + #motors_tri.pde + #motors_y6.pde + navigation.pde + planner.pde + radio.pde + #read_commands.pde + sensors.pde + setup.pde + system.pde + test.pde + ) # Firmware sketches + +#create dummy sourcefile +file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit") + +set(${FIRMWARE_NAME}_SRCS + #test.cpp + ${FIRMWARE_NAME}.cpp + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + APM_Config.h + APM_Config_mavlink_hil.h + APM_Config_xplane.h + config.h + defines.h + GCS.h + HIL.h + Mavlink_Common.h + Parameters.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + DataFlash + AP_Math + PID + RC_Channel + AP_OpticalFlow + ModeFilter + memcheck + #AP_PID + APM_PI + #APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + c + m +) +SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/DataFlash +${CMAKE_SOURCE_DIR}/libraries/AP_Math +${CMAKE_SOURCE_DIR}/libraries/PID +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/RC_Channel +${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/memcheck +#${CMAKE_SOURCE_DIR}/libraries/AP_PID +${CMAKE_SOURCE_DIR}/libraries/APM_PI +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +#new +#${CMAKE_SOURCE_DIR}/libraries/Wire +#${CMAKE_SOURCE_DIR}/libraries/SPI +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) diff --git a/cmake/PdeP.jar b/cmake/PdeP.jar new file mode 100644 index 0000000000000000000000000000000000000000..ae76f9b762e0726a1fc249243eadb49b3b69cd6f GIT binary patch literal 5690 zcmZ`-WmFVgw{@l9ZN`?o>)Z^6K~g zc;ENC_wIAfT4$ZLpS}0__dJg}7#)KYQyW!D{cb4t`>6jO1Lr@@Vd-4|8Uy$D ztE0Jvy7TYtV8;6=@vnJ%+W(zM_g{(s!8rcLSh%=2IP;jnp)M|&df1*uI%M}hxKj5v z6Z36~>~@t~zQh$I5a)nfddUl!Z0PfGQ$MS+@(zvNG?u~aU ztJB(L9XwyM&Ff&9hJ{Vjpj7h(OvKBRzF07$&WU1fJM3sC;%70vBaTR9el=skEK4mm zv8>|9Dw&wNeVd5&!D@51T-Ct=s%lRiV=gl;@~2ZzZPjEW;f53`66>ByOCN{i8EL?E zycGs;n4Ow}s7z*w`NGFJrwQ}24pGXY89+H*lJ(aDROY<*$NXmbKNXAK&WcAdm(83E zSZkZ}HF44KJK748I~Y>{MKJ(jimXRm`o!SgNGjCUvCgb%!_rG+v?d}X$13m2z*CZ!VhY0Ym^+qoELj_ttiTJNj?E}?X|v}ZXsa}njwQEMN`Jj zk$>ZHY;dTHV3j!m8D!~v%kv%zWhSAd*q&Srbo>y^X z<>SD6=RhUK_mv4!Q&Y6srX9FCJ>zE1*20yS(xXy3qH0 z<5qk=UgDRP9vd>Uxe9z)XJG}e4xNny`I&eHU~I8_FyiqL%lRn0&GN`XO=7|` z^dnPQ6Z+>GrZ>RN8*tBl}6+Pt(`BOL*d!(txJ&Gvn#Rz$z zw&RL3?y{2{z{u>e6{CYLW)sSS60>6%8+7-`+b}5bQ;S}8*f>g1)4cCgwZaft3v9IfyDJp1Q2J_L)N-!-;7OkCK zp}85*jKFFle7$at!{~&s-YOPhR7^Qa*gxHkD2rE;DN2k#!xdQj$&B~rv+M#OiWMaL zvgg4@YFTKg4i*zqxk}DZ#ho;&ZW4^Gt){u$Ko&ROtkQEdH;lmeA0-uf2n5#8mh`K7|f2Uv1 zTDzH^;DeM2i_A(2GfIwB5@Nv*Kn_49Oy#tzV6Cyyn~)9S>;Us^^NA>?ZM;J2%ZVgM zmR)&*>};MlI>O_N#B+dS?$@N-{Wd!O^*^z?134sP$~e%6EpL`~^O%i04C1%}s;)ZM z=>7!wYgM=ePuldAUDAFww}3x`|HWiFVWo@{`TqxKQZo4#fX!}s3OdZ(FqPmAgb&De#qb|!@?ooUhU zK`)|=JNO>8H5YIJmKVwJLAw^>zuu<`gFp|ALH4(CcR9ne#G@FN1|_!wKBKSpCcsqw zL3Z#HKLygkGVNDb_trm&mn>gKa$e;4oVv-9ui8xjJcP6Shg z-`?OTsux(!J+o2Z7USFIzA;{BpP*1QcE(pg2iGEC8GsMRuZc_BNuwqPF!zRPUt1+WQMo1e>6t1i4u|y4uPA{jqFE@?K zv^8uC!=Um&X+tXiL!|U!x27+e?J>Kkk3ihj6OUPrMv63i1N-5`$QpG{N~?6kdKZhG z_PyPgk*H1^rf?7=PgY(qa8s0OSFUZeV;V9Q<c{caO3i^MEn%xom@ZD7+Z%$~XI*ci z+pUo_a|5xtWFA2Ir2$8i-R`BxTM6-BU0C(bsN z*!=KuVq_R~9>*N5q%i2kxAg zPWsj!4Ov`>@au{b`&B~6&&vib>OK$;ZiNG=IS|Q=^V&(A+Ji?20Wb7<-unifqbpa| zRY$I&TA@;8<+rO%jOOJ__B;hxR9*oER}Cy&qHNSEj0E}0Hxiyv>s-TO%+#5gHKSJD zxW!N$a((O;p`FG}4yIY1QXn7M)cX`Ui0WG2x_@Cfwyj-N6d4;mFIyf<8bTy5*dhVh zSS?90iLR`jm`h?YqH%Ojva!wC5=S zwGy?Dmx)eX*DTvyz~8WVVbodKI6GJNrHo2NRe3t$AF$t zO8aX*qiTwx!-mWF{w+6Mu@TyD5uJwz0kX>)^_TmPlFq4v{2JshUI5YUfh4p}RG-5| zp7L(jaBSzix^irJY?8xP)K8|*&avl4HuJ%%T|YFmm^SXBA>-06>Czr;RYj1*02|t^ z=ss3G*Xx|zXE2iTq%iaT@@#Fm;Ld_R<eS z%?}HushXJ1Mtn-ddS+C{W9)U=<&jJP{QP4PI`9E$_}<_$!)J8)=J4*w;fG7*-H&!o zl#BH-Dqpm%y(WO;Rjt_ss;WQ&4m^wBYQ%c-Rg}U_Ka0f(8Jk`0h9=##i;wNN+ox$y z+)E=7aO{5T1-@muxsfR81RoHT7#EF9YSW&nkDKVaWw`u_UY(y1BNrj^`W`CU{260)SB^S|9M3ojM;e-a`swAIlG4u@~!ADi@8Qf{^XL4T|>XYh6^UN|eU3vz+RKQuRzSx<@u9jeDTAL5)gB*mMGLBhCkd$NniNI*p)nM1e|co+ z_|^+JWs}MP!?Vq4;BiU_>whBtU2bFHmkOC_SzwSg$(%6M@>(~S$t~L9C6=fFz>sFT zB$BfTPdtoASqUNjr1GWc%REfQpk4XO5Zcr82KRpWt3u;pRmxqhER6nwZ=4`Xmkc;1 z5{j42HBc#NWkN`lo1zOIOmMyp4TQ>R2XNU~?bZbFL)lqyn%5`yK}t<3K^2tg`vOz6 zQUc=bL#;X3{V!6BT=wd03NT7x@-w+1(%S~;g^0jRH@9947}kOq1CwVOhLvIiN$UJk z6YC`5UTtBVCCj8u|Aw=n;p9XY34WcB0v6)tX}$z|krCyWG?0@BW4yb!Vd3KLhmj^T zK3Y4s^?5&jnn-;qEn*t)NEpE$v&H!bW&TAT6R;K&D~%UkoaDp;*p!WrMOk|kpe!Vk~*bX z?q_J?6Jw{Q!5l|tf+Mx1#A4b4d+^HjHm|C$%@fHLmEXwUljU=t2~*O!JR%pcr`A^O zG+CANjoyo|O@h@67?;NQLXd3sPo*-tRDXKTBRQ{fks54fI z?X+0+|JvTs!}1M`?~ULPBBga8>ghVvpGXV&LEIxIYD@;OjB{T7(Kotv|m5Xh_i>AB1iR0%c?@=Ffk6sB>lph z`1ZvdT5^4hamD3NoAw!APgWuMXpNx!N;M;Al<_ptU*4}!Fpq!j`;uBw0d?499;r^K zje5?uv-6GQSn5tEM9^6JmxVUqb#D^5m^iuWh+)=mZqzE&5Xk~d$N{8Rzfbyp;w~k& zMrNxKee3wP)1FAoz6`6auPr>50@>a)_L|WlIq{*d$XUXPk^s;eiHM#UP>_CREX(be z+8=~lta;CE@*2N2smg~)B`0R5N)n-V>^1#5!1VjfPcK<-@^MIZkCw=}&#=tdLC0o2CNG@|3O z2{>vv#!TP&;9LlPzF-HK!TW#dh9GBdS_E7PU{5FRUiD#%$H%m8W@C7u=jU@0bO! z*-VzAL2Me zp1}?=r-F+J;foDS{tcC+egpSVkMU^S#pYmDt{mKeF0Cuh>l0MTj*5-v_Yw3n=R_yZ zL*zIg$r8x-LNf@-<@<&`bu)HuT5r9Z-vu{?V>{u+pYyO+Ff!jq?f8jUo}j4yrnJH)s!Dxaop~~1R^|Q&3u!4srBG?;hDQeb#{32KoF7CKgsARm~d-y{f zTHHAqFR)Nh(DDBe2UIjtl)qike;Sp4T+x5W|L2kZse*&@|C^eBS3&$29r;K5KPKlt zISt+4sNc7${{sGBmghe|^M?oh^P_)w(BJXx$-kY}-_`zNMe1P8M}N{VelJ2)6cp0` G;Qj|rIcQP< literal 0 HcmV?d00001 diff --git a/cmake/modules/ArduinoProcessing.cmake b/cmake/modules/ArduinoProcessing.cmake new file mode 100644 index 0000000000..c7a2407a2a --- /dev/null +++ b/cmake/modules/ArduinoProcessing.cmake @@ -0,0 +1,103 @@ +# 1. Concatenate all PDE files +# 2. Write #include "WProgram.h" +# 3. Write prototypes +# 4. Write original sources +# +# +# Prefix Writer +# 1. Scrub comments +# 2. Optionally subsitute Unicode +# 3. Find imports +# 4. Find prototypes +# +# Find prototypes +# 1. Strip comments, quotes, preprocessor directives +# 2. Collapse braches +# 3. Regex + + +set(SINGLE_QUOTES_REGEX "('.')") +set(DOUBLE_QUOTES_REGEX "(\"([^\"\\\\]|\\\\.)*\")") +set(SINGLE_COMMENT_REGEX "([ ]*//[^\n]*)") +set(MULTI_COMMENT_REGEX "(/[*][^/]*[*]/)") +set(PREPROC_REGEX "([ ]*#(\\\\[\n]|[^\n])*)") + +#"[\w\[\]\*]+\s+[&\[\]\*\w\s]+\([&,\[\]\*\w\s]*\)(?=\s*\{)" +set(PROTOTPYE_REGEX "([a-zA-Z0-9]+[ ]*)*[a-zA-Z0-9]+[ ]*\([^{]*\)[ ]*{") + +function(READ_SKETCHES VAR_NAME ) + set(SKETCH_SOURCE) + foreach(SKETCH ${ARGN}) + if(EXISTS ${SKETCH}) + message(STATUS "${SKETCH}") + file(READ ${SKETCH} SKETCH_CONTENTS) + set(SKETCH_SOURCE "${SKETCH_SOURCE}\n${SKETCH_CONTENTS}") + else() + message(FATAL_ERROR "Sketch file does not exist: ${SKETCH}") + endif() + endforeach() + set(${VAR_NAME} "${SKETCH_SOURCE}" PARENT_SCOPE) +endfunction() + +function(STRIP_SOURCES VAR_NAME SOURCES) + string(REGEX REPLACE "${SINGLE_QUOTES_REGEX}|${DOUBLE_QUOTES_REGEX}|${SINGLE_COMMENT_REGEX}|${MULTI_COMMENT_REGEX}|${PREPROC_REGEX}" + "" + SOURCES + "${SOURCES}") + set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE) +endfunction() + +function(COLLAPSE_BRACES VAR_NAME SOURCES) + set(PARSED_SOURCES) + string(LENGTH "${SOURCES}" SOURCES_LENGTH) + math(EXPR SOURCES_LENGTH "${SOURCES_LENGTH}-1") + + set(NESTING 0) + set(START 0) + foreach(INDEX RANGE ${SOURCES_LENGTH}) + string(SUBSTRING "${SOURCES}" ${INDEX} 1 CURRENT_CHAR) + #message("${CURRENT_CHAR}") + if(CURRENT_CHAR STREQUAL "{") + if(NESTING EQUAL 0) + math(EXPR SUBLENGTH "${INDEX}-${START} +1") + string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK) + set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}") + #message("INDEX: ${INDEX} START: ${START} LENGTH: ${SUBLENGTH}") + endif() + math(EXPR NESTING "${NESTING}+1") + elseif(CURRENT_CHAR STREQUAL "}") + math(EXPR NESTING "${NESTING}-1") + if(NESTING EQUAL 0) + set(START ${INDEX}) + endif() + endif() + endforeach() + + math(EXPR SUBLENGTH "${SOURCES_LENGTH}-${START} +1") + string(SUBSTRING "${SOURCES}" ${START} ${SUBLENGTH} CURRENT_CHUNK) + set(PARSED_SOURCES "${PARSED_SOURCES}${CURRENT_CHUNK}") + + set(${VAR_NAME} "${PARSED_SOURCES}" PARENT_SCOPE) +endfunction() + +function(extract_prototypes VAR_NAME SOURCES) + string(REGEX MATCHALL "${PROTOTPYE_REGEX}" + SOURCES + "${SOURCES}") + set(${VAR_NAME} "${SOURCES}" PARENT_SCOPE) +endfunction() + +read_sketches(SKETCH_SOURCE ${FILES}) +strip_sources(SKETCH_SOURCE "${SKETCH_SOURCE}") +collapse_braces(SKETCH_SOURCE "${SKETCH_SOURCE}") +extract_prototypes(SKETCH_SOURCE "${SKETCH_SOURCE}") + + + + +message("===============") +foreach(ENTRY ${SKETCH_SOURCE}) + message("START]]]${ENTRY}[[[END") +endforeach() +message("===============") +#message("${SKETCH_SOURCE}") diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake new file mode 100644 index 0000000000..7faadeba15 --- /dev/null +++ b/cmake/modules/FindArduino.cmake @@ -0,0 +1,581 @@ +# - Generate firmware and libraries for Arduino Devices +# generate_arduino_firmware(TARGET_NAME) +# TARGET_NAME - Name of target +# Creates a Arduino firmware target. +# +# The target options can be configured by setting options of +# the following format: +# ${TARGET_NAME}${SUFFIX} +# The following suffixes are availabe: +# _SRCS # Sources +# _HDRS # Headers +# _SKETCHES # Arduino sketch files +# _LIBS # Libraries to linked in +# _BOARD # Board name (such as uno, mega2560, ...) +# _PORT # Serial port, for upload and serial targets [OPTIONAL] +# _AFLAGS # Override global Avrdude flags for target +# _SERIAL # Serial command for serial target [OPTIONAL] +# _NO_AUTOLIBS # Disables Arduino library detection +# Here is a short example for a target named test: +# set(test_SRCS test.cpp) +# set(test_HDRS test.h) +# set(test_BOARD uno) +# +# generate_arduino_firmware(test) +# +# +# generate_arduino_library(TARGET_NAME) +# TARGET_NAME - Name of target +# Creates a Arduino firmware target. +# +# The target options can be configured by setting options of +# the following format: +# ${TARGET_NAME}${SUFFIX} +# The following suffixes are availabe: +# +# _SRCS # Sources +# _HDRS # Headers +# _LIBS # Libraries to linked in +# _BOARD # Board name (such as uno, mega2560, ...) +# _NO_AUTOLIBS # Disables Arduino library detection +# +# Here is a short example for a target named test: +# set(test_SRCS test.cpp) +# set(test_HDRS test.h) +# set(test_BOARD uno) +# +# generate_arduino_library(test) + + +find_path(ARDUINO_SDK_PATH + NAMES lib/version.txt hardware libraries + PATH_SUFFIXES share/arduino + DOC "Arduino Development Kit path.") + + +# load_board_settings() +# +# Load the Arduino SDK board settings from the boards.txt file. +# +function(LOAD_BOARD_SETTINGS) + if(NOT ARDUINO_BOARDS AND ARDUINO_BOARDS_PATH) + file(STRINGS ${ARDUINO_BOARDS_PATH} BOARD_SETTINGS) + foreach(BOARD_SETTING ${BOARD_SETTINGS}) + if("${BOARD_SETTING}" MATCHES "^.*=.*") + string(REGEX MATCH "^[^=]+" SETTING_NAME ${BOARD_SETTING}) + string(REGEX MATCH "[^=]+$" SETTING_VALUE ${BOARD_SETTING}) + string(REPLACE "." ";" SETTING_NAME_TOKENS ${SETTING_NAME}) + + list(LENGTH SETTING_NAME_TOKENS SETTING_NAME_TOKENS_LEN) + + + # Add Arduino to main list of arduino boards + list(GET SETTING_NAME_TOKENS 0 BOARD_ID) + list(FIND ARDUINO_BOARDS ${BOARD_ID} ARDUINO_BOARD_INDEX) + if(ARDUINO_BOARD_INDEX LESS 0) + list(APPEND ARDUINO_BOARDS ${BOARD_ID}) + endif() + + # Add setting to board settings list + list(GET SETTING_NAME_TOKENS 1 BOARD_SETTING) + list(FIND ${BOARD_ID}.SETTINGS ${BOARD_SETTING} BOARD_SETTINGS_LEN) + if(BOARD_SETTINGS_LEN LESS 0) + list(APPEND ${BOARD_ID}.SETTINGS ${BOARD_SETTING}) + set(${BOARD_ID}.SETTINGS ${${BOARD_ID}.SETTINGS} + CACHE INTERNAL "Arduino ${BOARD_ID} Board settings list") + endif() + + set(ARDUINO_SETTING_NAME ${BOARD_ID}.${BOARD_SETTING}) + + # Add sub-setting to board sub-settings list + if(SETTING_NAME_TOKENS_LEN GREATER 2) + list(GET SETTING_NAME_TOKENS 2 BOARD_SUBSETTING) + list(FIND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING} BOARD_SUBSETTINGS_LEN) + if(BOARD_SUBSETTINGS_LEN LESS 0) + list(APPEND ${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${BOARD_SUBSETTING}) + set(${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS ${${BOARD_ID}.${BOARD_SETTING}.SUBSETTINGS} + CACHE INTERNAL "Arduino ${BOARD_ID} Board sub-settings list") + endif() + set(ARDUINO_SETTING_NAME ${ARDUINO_SETTING_NAME}.${BOARD_SUBSETTING}) + endif() + + # Save setting value + set(${ARDUINO_SETTING_NAME} ${SETTING_VALUE} CACHE INTERNAL "Arduino ${BOARD_ID} Board setting") + + + endif() + endforeach() + set(ARDUINO_BOARDS ${ARDUINO_BOARDS} CACHE STRING "List of detected Arduino Board configurations") + mark_as_advanced(ARDUINO_BOARDS) + endif() +endfunction() + +# print_board_settings(ARDUINO_BOARD) +# +# ARDUINO_BOARD - Board id +# +# Print the detected Arduino board settings. +# +function(PRINT_BOARD_SETTINGS ARDUINO_BOARD) + if(${ARDUINO_BOARD}.SETTINGS) + + message(STATUS "Arduino ${ARDUINO_BOARD} Board:") + foreach(BOARD_SETTING ${${ARDUINO_BOARD}.SETTINGS}) + if(${ARDUINO_BOARD}.${BOARD_SETTING}) + message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}}") + endif() + if(${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS) + foreach(BOARD_SUBSETTING ${${ARDUINO_BOARD}.${BOARD_SETTING}.SUBSETTINGS}) + if(${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}) + message(STATUS " ${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}=${${ARDUINO_BOARD}.${BOARD_SETTING}.${BOARD_SUBSETTING}}") + endif() + endforeach() + endif() + message(STATUS "") + endforeach() + endif() +endfunction() + + + +# generate_arduino_library(TARGET_NAME) +# +# see documentation at top +function(GENERATE_ARDUINO_LIBRARY TARGET_NAME) + load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources + _HDRS # Headers + _LIBS # Libraries to linked in + _BOARD) # Board name (such as uno, mega2560, ...) + set(INPUT_AUTOLIBS True) + if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS) + set(INPUT_AUTOLIBS False) + endif() + + message(STATUS "Generating ${TARGET_NAME}") + + set(ALL_LIBS) + set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS}) + + setup_arduino_compiler(${INPUT_BOARD}) + setup_arduino_core(CORE_LIB ${INPUT_BOARD}) + + if(INPUT_AUTOLIBS) + setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}") + endif() + + list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS}) + + add_library(${TARGET_NAME} ${ALL_SRCS}) + target_link_libraries(${TARGET_NAME} ${ALL_LIBS}) +endfunction() + +# generate_arduino_firmware(TARGET_NAME) +# +# see documentation at top +function(GENERATE_ARDUINO_FIRMWARE TARGET_NAME) + load_generator_settings(${TARGET_NAME} INPUT _SRCS # Sources + _HDRS # Headers + _LIBS # Libraries to linked in + _BOARD # Board name (such as uno, mega2560, ...) + _PORT # Serial port, for upload and serial targets + _AFLAGS # Override global Avrdude flags for target + _SKETCHES # Arduino sketch files + _SERIAL) # Serial command for serial target + + set(INPUT_AUTOLIBS True) + if(DEFINED ${TARGET_NAME}_NO_AUTOLIBS AND ${TARGET_NAME}_NO_AUTOLIBS) + set(INPUT_AUTOLIBS False) + endif() + + message(STATUS "Generating ${TARGET_NAME}") + message(STATUS "Sketches ${INPUT_SKETCHES}") + + set(ALL_LIBS) + set(ALL_SRCS ${INPUT_SKETCHES} ${INPUT_SRCS} ${INPUT_HDRS} ) + #set(ALL_SRCS ${INPUT_SRCS} ${INPUT_HDRS} ) + + #set compile flags and file properties for pde files + #SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES LANGUAGE CXX) + #SET_SOURCE_FILES_PROPERTIES(${INPUT_SKETCHES} PROPERTIES COMPILE_FLAGS "-x c++" ) + + setup_arduino_compiler(${INPUT_BOARD}) + setup_arduino_core(CORE_LIB ${INPUT_BOARD}) + + #setup_arduino_sketch(SKETCH_SRCS ${INPUT_SKETCHES}) + + if(INPUT_AUTOLIBS) + setup_arduino_libraries(ALL_LIBS ${INPUT_BOARD} "${ALL_SRCS}") + endif() + + + list(APPEND ALL_LIBS ${CORE_LIB} ${INPUT_LIBS}) + + setup_arduino_target(${TARGET_NAME} "${ALL_SRCS}" "${ALL_LIBS}") + #SET_TARGET_PROPERTIES(${TARGET_NAME} PROPERTIES LINKER_LANGUAGE CXX) + #setup_arduino_target(${TARGET_NAME} "${INPUT_SKETCHES}" ${INPUT_HDRS} "${ALL_LIBS}") + + if(INPUT_PORT) + setup_arduino_upload(${INPUT_BOARD} ${TARGET_NAME} ${INPUT_PORT}) + endif() + + if(INPUT_SERIAL) + setup_serial_target(${TARGET_NAME} "${INPUT_SERIAL}") + endif() +endfunction() + + +# load_generator_settings(TARGET_NAME PREFIX [SUFFIX_1 SUFFIX_2 .. SUFFIX_N]) +# +# TARGET_NAME - The base name of the user settings +# PREFIX - The prefix name used for generator settings +# SUFFIX_XX - List of suffixes to load +# +# Loads a list of user settings into the generators scope. User settings have +# the following syntax: +# +# ${BASE_NAME}${SUFFIX} +# +# The BASE_NAME is the target name and the suffix is a specific generator settings. +# +# For every user setting found a generator setting is created of the follwoing fromat: +# +# ${PREFIX}${SUFFIX} +# +# The purpose of loading the settings into the generator is to not modify user settings +# and to have a generic naming of the settings within the generator. +# +function(LOAD_GENERATOR_SETTINGS TARGET_NAME PREFIX) + foreach(GEN_SUFFIX ${ARGN}) + if(${TARGET_NAME}${GEN_SUFFIX}) + set(${PREFIX}${GEN_SUFFIX} ${${TARGET_NAME}${GEN_SUFFIX}} PARENT_SCOPE) + endif() + endforeach() +endfunction() + +# setup_arduino_compiler(BOARD_ID) +# +# BOARD_ID - The board id name +# +# Configures the the build settings for the specified Arduino Board. +# +macro(setup_arduino_compiler BOARD_ID) + set(BOARD_CORE ${${BOARD_ID}.build.core}) + if(BOARD_CORE) + set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE}) + include_directories(${BOARD_CORE_PATH}) + include_directories(${ARDUINO_LIBRARIES_PATH}) + add_definitions(-DF_CPU=${${BOARD_ID}.build.f_cpu} + -DARDUINO=${ARDUINO_SDK_VERSION} + -mmcu=${${BOARD_ID}.build.mcu} + ) + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -mmcu=${${BOARD_ID}.build.mcu}" PARENT_SCOPE) + endif() +endmacro() + +# setup_arduino_core(VAR_NAME BOARD_ID) +# +# VAR_NAME - Variable name that will hold the generated library name +# BOARD_ID - Arduino board id +# +# Creates the Arduino Core library for the specified board, +# each board gets it's own version of the library. +# +function(setup_arduino_core VAR_NAME BOARD_ID) + set(CORE_LIB_NAME ${BOARD_ID}_CORE) + set(BOARD_CORE ${${BOARD_ID}.build.core}) + if(BOARD_CORE AND NOT TARGET ${CORE_LIB_NAME}) + set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE}) + find_sources(CORE_SRCS ${BOARD_CORE_PATH}) + add_library(${CORE_LIB_NAME} ${CORE_SRCS}) + set(${VAR_NAME} ${CORE_LIB_NAME} PARENT_SCOPE) + endif() +endfunction() + +# find_arduino_libraries(VAR_NAME SRCS) +# +# VAR_NAME - Variable name which will hold the results +# SRCS - Sources that will be analized +# +# returns a list of paths to libraries found. +# +# Finds all Arduino type libraries included in sources. Available libraries +# are ${ARDUINO_SDK_PATH}/libraries and ${CMAKE_CURRENT_SOURCE_DIR}. +# +# A Arduino library is a folder that has the same name as the include header. +# For example, if we have a include "#include " then the following +# directory structure is considered a Arduino library: +# +# LibraryName/ +# |- LibraryName.h +# `- LibraryName.c +# +# If such a directory is found then all sources within that directory are considred +# to be part of that Arduino library. +# +function(find_arduino_libraries VAR_NAME SRCS) + set(ARDUINO_LIBS ) + foreach(SRC ${SRCS}) + file(STRINGS ${SRC} SRC_CONTENTS) + foreach(SRC_LINE ${SRC_CONTENTS}) + if("${SRC_LINE}" MATCHES "^ *#include *[<\"](.*)[>\"]") + get_filename_component(INCLUDE_NAME ${CMAKE_MATCH_1} NAME_WE) + foreach(LIB_SEARCH_PATH ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR}) + if(EXISTS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}/${CMAKE_MATCH_1}) + list(APPEND ARDUINO_LIBS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}) + break() + endif() + endforeach() + endif() + endforeach() + endforeach() + if(ARDUINO_LIBS) + list(REMOVE_DUPLICATES ARDUINO_LIBS) + endif() + set(${VAR_NAME} ${ARDUINO_LIBS} PARENT_SCOPE) +endfunction() + +# setup_arduino_library(VAR_NAME BOARD_ID LIB_PATH) +# +# VAR_NAME - Vairable wich will hold the generated library names +# BOARD_ID - Board name +# LIB_PATH - path of the library +# +# Creates an Arduino library, with all it's library dependencies. +# +function(setup_arduino_library VAR_NAME BOARD_ID LIB_PATH) + set(LIB_TARGETS) + get_filename_component(LIB_NAME ${LIB_PATH} NAME) + set(TARGET_LIB_NAME ${BOARD_ID}_${LIB_NAME}) + if(NOT TARGET ${TARGET_LIB_NAME}) + find_sources(LIB_SRCS ${LIB_PATH}) + if(LIB_SRCS) + + message(STATUS "Generating Arduino ${LIB_NAME} library") + include_directories(${LIB_PATH} ${LIB_PATH}/utility) + add_library(${TARGET_LIB_NAME} STATIC ${LIB_SRCS}) + + find_arduino_libraries(LIB_DEPS "${LIB_SRCS}") + foreach(LIB_DEP ${LIB_DEPS}) + setup_arduino_library(DEP_LIB_SRCS ${BOARD_ID} ${LIB_DEP}) + list(APPEND LIB_TARGETS ${DEP_LIB_SRCS}) + endforeach() + + target_link_libraries(${TARGET_LIB_NAME} ${BOARD_ID}_CORE ${LIB_TARGETS}) + list(APPEND LIB_TARGETS ${TARGET_LIB_NAME}) + endif() + else() + # Target already exists, skiping creating + include_directories(${LIB_PATH} ${LIB_PATH}/utility) + list(APPEND LIB_TARGETS ${TARGET_LIB_NAME}) + endif() + if(LIB_TARGETS) + list(REMOVE_DUPLICATES LIB_TARGETS) + endif() + set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE) +endfunction() + +# setup_arduino_libraries(VAR_NAME BOARD_ID SRCS) +# +# VAR_NAME - Vairable wich will hold the generated library names +# BOARD_ID - Board ID +# SRCS - source files +# +# Finds and creates all dependency libraries based on sources. +# +function(setup_arduino_libraries VAR_NAME BOARD_ID SRCS) + set(LIB_TARGETS) + find_arduino_libraries(TARGET_LIBS ${SRCS}) + foreach(TARGET_LIB ${TARGET_LIBS}) + setup_arduino_library(LIB_DEPS ${BOARD_ID} ${TARGET_LIB}) # Create static library instead of returning sources + list(APPEND LIB_TARGETS ${LIB_DEPS}) + endforeach() + set(${VAR_NAME} ${LIB_TARGETS} PARENT_SCOPE) +endfunction() + + +# setup_arduino_target(TARGET_NAME ALL_SRCS ALL_LIBS) +# +# TARGET_NAME - Target name +# ALL_SRCS - All sources +# ALL_LIBS - All libraries +# +# Creates an Arduino firmware target. +# +function(setup_arduino_target TARGET_NAME ALL_SRCS ALL_LIBS) + add_executable(${TARGET_NAME} ${ALL_SRCS}) + target_link_libraries(${TARGET_NAME} ${ALL_LIBS}) + set_target_properties(${TARGET_NAME} PROPERTIES SUFFIX ".elf") + + set(TARGET_PATH ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}) + add_custom_command(TARGET ${TARGET_NAME} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} + ARGS ${ARDUINO_OBJCOPY_EEP_FLAGS} + ${TARGET_PATH}.elf + ${TARGET_PATH}.eep + VERBATIM) + add_custom_command(TARGET ${TARGET_NAME} POST_BUILD + COMMAND ${CMAKE_OBJCOPY} + ARGS ${ARDUINO_OBJCOPY_HEX_FLAGS} + ${TARGET_PATH}.elf + ${TARGET_PATH}.hex + VERBATIM) +endfunction() + +# setup_arduino_upload(BOARD_ID TARGET_NAME PORT) +# +# BOARD_ID - Arduino board id +# TARGET_NAME - Target name +# PORT - Serial port for upload +# +# Create an upload target (${TARGET_NAME}-upload) for the specified Arduino target. +# +function(setup_arduino_upload BOARD_ID TARGET_NAME PORT) + set(AVRDUDE_FLAGS ${ARDUINO_AVRDUDE_FLAGS}) + if(DEFINED ${TARGET_NAME}_AFLAGS) + set(AVRDUDE_FLAGS ${${TARGET_NAME}_AFLAGS}) + endif() + add_custom_target(${TARGET_NAME}-upload + ${ARDUINO_AVRDUDE_PROGRAM} + -U flash:w:${TARGET_NAME}.hex + ${AVRDUDE_FLAGS} + -C ${ARDUINO_AVRDUDE_CONFIG_PATH} + -p ${${BOARD_ID}.build.mcu} + -c ${${BOARD_ID}.upload.protocol} + -b ${${BOARD_ID}.upload.speed} + -P ${PORT} + DEPENDS ${TARGET_NAME}) + if(NOT TARGET upload) + add_custom_target(upload) + endif() + add_dependencies(upload ${TARGET_NAME}-upload) +endfunction() + +# find_sources(VAR_NAME LIB_PATH) +# +# VAR_NAME - Variable name that will hold the detected sources +# LIB_PATH - The base path +# +# Finds all C/C++ sources located at the specified path. +# +function(find_sources VAR_NAME LIB_PATH) + file(GLOB_RECURSE LIB_FILES ${LIB_PATH}/*.cpp + ${LIB_PATH}/*.c + ${LIB_PATH}/*.cc + ${LIB_PATH}/*.cxx + ${LIB_PATH}/*.h + ${LIB_PATH}/*.hh + ${LIB_PATH}/*.hxx) + if(LIB_FILES) + set(${VAR_NAME} ${LIB_FILES} PARENT_SCOPE) + endif() +endfunction() + +# setup_serial_target(TARGET_NAME CMD) +# +# TARGET_NAME - Target name +# CMD - Serial terminal command +# +# Creates a target (${TARGET_NAME}-serial) for launching the serial termnial. +# +function(setup_serial_target TARGET_NAME CMD) + string(CONFIGURE "${CMD}" FULL_CMD @ONLY) + add_custom_target(${TARGET_NAME}-serial + ${FULL_CMD}) +endfunction() + + +# detect_arduino_version(VAR_NAME) +# +# VAR_NAME - Variable name where the detected version will be saved +# +# Detects the Arduino SDK Version based on the revisions.txt file. +# +function(detect_arduino_version VAR_NAME) + if(ARDUINO_VERSION_PATH) + file(READ ${ARDUINO_VERSION_PATH} ARD_VERSION) + if("${ARD_VERSION}" MATCHES " *[0]+([0-9]+)") + set(${VAR_NAME} ${CMAKE_MATCH_1} PARENT_SCOPE) + endif() + endif() +endfunction() + + +function(convert_arduino_sketch VAR_NAME SRCS) +endfunction() + + +# Setting up Arduino enviroment settings +if(NOT ARDUINO_FOUND) + find_file(ARDUINO_CORES_PATH + NAMES cores + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_LIBRARIES_PATH + NAMES libraries + PATHS ${ARDUINO_SDK_PATH}) + + find_file(ARDUINO_BOARDS_PATH + NAMES boards.txt + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_PROGRAMMERS_PATH + NAMES programmers.txt + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/arduino) + + find_file(ARDUINO_REVISIONS_PATH + NAMES revisions.txt + PATHS ${ARDUINO_SDK_PATH}) + + find_file(ARDUINO_VERSION_PATH + NAMES lib/version.txt + PATHS ${ARDUINO_SDK_PATH}) + + find_program(ARDUINO_AVRDUDE_PROGRAM + NAMES avrdude + PATHS ${ARDUINO_SDK_PATH} + PATH_SUFFIXES hardware/tools) + + find_program(ARDUINO_AVRDUDE_CONFIG_PATH + NAMES avrdude.conf + PATHS ${ARDUINO_SDK_PATH} /etc/avrdude + PATH_SUFFIXES hardware/tools + hardware/tools/avr/etc) + + set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0 + CACHE STRING "") + set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom + CACHE STRING "") + set(ARDUINO_AVRDUDE_FLAGS -V -F + CACHE STRING "Arvdude global flag list.") + + if(ARDUINO_SDK_PATH) + detect_arduino_version(ARDUINO_SDK_VERSION) + set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version") + endif(ARDUINO_SDK_PATH) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Arduino + REQUIRED_VARS ARDUINO_SDK_PATH + ARDUINO_SDK_VERSION + VERSION_VAR ARDUINO_SDK_VERSION) + + + mark_as_advanced(ARDUINO_CORES_PATH + ARDUINO_SDK_VERSION + ARDUINO_LIBRARIES_PATH + ARDUINO_BOARDS_PATH + ARDUINO_PROGRAMMERS_PATH + ARDUINO_REVISIONS_PATH + ARDUINO_AVRDUDE_PROGRAM + ARDUINO_AVRDUDE_CONFIG_PATH + ARDUINO_OBJCOPY_EEP_FLAGS + ARDUINO_OBJCOPY_HEX_FLAGS) + load_board_settings() + +endif() + diff --git a/cmake/modules/Platform/Arduino.cmake b/cmake/modules/Platform/Arduino.cmake new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cmake/modules/Platform/ArduinoPaths.cmake b/cmake/modules/Platform/ArduinoPaths.cmake new file mode 100644 index 0000000000..27372098cc --- /dev/null +++ b/cmake/modules/Platform/ArduinoPaths.cmake @@ -0,0 +1,21 @@ +if(UNIX) + include(Platform/UnixPaths) + if(APPLE) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ~/Applications + /Applications + /Developer/Applications + /sw # Fink + /opt/local) # MacPorts + endif() +elseif(WIN32) + include(Platform/WindowsPaths) +endif() + +if(ARDUINO_SDK_PATH) + if(WIN32) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/utils/bin) + elseif(APPLE) + list(APPEND CMAKE_SYSTEM_PREFIX_PATH ${ARDUINO_SDK_PATH}/hardware/tools/avr/bin) + endif() +endif() diff --git a/cmake/toolchains/Arduino.cmake b/cmake/toolchains/Arduino.cmake new file mode 100644 index 0000000000..616c7b9160 --- /dev/null +++ b/cmake/toolchains/Arduino.cmake @@ -0,0 +1,66 @@ +set(CMAKE_SYSTEM_NAME Arduino) + +set(CMAKE_C_COMPILER avr-gcc) +set(CMAKE_CXX_COMPILER avr-g++) + +#=============================================================================# +# C Flags # +#=============================================================================# +set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections") +set(CMAKE_C_FLAGS "-g -Os ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_DEBUG "-g ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_RELEASE "-Os -DNDEBUG -w ${ARDUINO_C_FLAGS}" CACHE STRING "") +set(CMAKE_C_FLAGS_RELWITHDEBINFO "-Os -g -w ${ARDUINO_C_FLAGS}" CACHE STRING "") + +#=============================================================================# +# C++ Flags # +#=============================================================================# +set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions") +set(CMAKE_CXX_FLAGS "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_DEBUG "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_MINSIZEREL "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_RELEASE "-Os -DNDEBUG ${ARDUINO_CXX_FLAGS}" CACHE STRING "") +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-Os -g ${ARDUINO_CXX_FLAGS}" CACHE STRING "") + +#=============================================================================# +# Executable Linker Flags # +#=============================================================================# +set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections -lc -lm") +set(CMAKE_EXE_LINKER_FLAGS "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_MINSIZEREL "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_RELEASE "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") +set(CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO "${ARDUINO_LINKER_FLAGS}" CACHE STRING "") + +#=============================================================================# +# Shared Lbrary Linker Flags # +#=============================================================================# +set(CMAKE_SHARED_LINKER_FLAGS "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "" CACHE STRING "") +set(CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "") + +set(CMAKE_MODULE_LINKER_FLAGS "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_DEBUG "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_RELEASE "" CACHE STRING "") +set(CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO "" CACHE STRING "") + + + + +set(ARDUINO_PATHS) +foreach(VERSION RANGE 22 1) + list(APPEND ARDUINO_PATHS arduino-00${VERSION}) +endforeach() + +find_path(ARDUINO_SDK_PATH + NAMES lib/version.txt + PATH_SUFFIXES share/arduino + Arduino.app/Contents/Resources/Java/ + ${ARDUINO_PATHS} + DOC "Arduino Development Kit path.") + +include(Platform/ArduinoPaths) diff --git a/libraries/APM_BMP085/CMakeLists.txt b/libraries/APM_BMP085/CMakeLists.txt new file mode 100644 index 0000000000..342fe4668f --- /dev/null +++ b/libraries/APM_BMP085/CMakeLists.txt @@ -0,0 +1,22 @@ +set(LIB_NAME APM_BMP085) + +set(${LIB_NAME}_SRCS + APM_BMP085.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_BMP085.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_PI/CMakeLists.txt b/libraries/APM_PI/CMakeLists.txt new file mode 100644 index 0000000000..e65e76f6a4 --- /dev/null +++ b/libraries/APM_PI/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME APM_PI) + +set(${LIB_NAME}_SRCS + APM_PI.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_PI.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_RC/CMakeLists.txt b/libraries/APM_RC/CMakeLists.txt new file mode 100644 index 0000000000..3e4aab0011 --- /dev/null +++ b/libraries/APM_RC/CMakeLists.txt @@ -0,0 +1,22 @@ +set(LIB_NAME APM_RC) + +set(${LIB_NAME}_SRCS + APM_RC.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + APM_RC.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_ADC/CMakeLists.txt b/libraries/AP_ADC/CMakeLists.txt new file mode 100644 index 0000000000..df85645fb2 --- /dev/null +++ b/libraries/AP_ADC/CMakeLists.txt @@ -0,0 +1,26 @@ +set(LIB_NAME AP_ADC) + +set(${LIB_NAME}_SRCS + AP_ADC_HIL.cpp + AP_ADC_ADS7844.cpp + AP_ADC.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_ADC_HIL.h + AP_ADC_ADS7844.h + AP_ADC.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Common/CMakeLists.txt b/libraries/AP_Common/CMakeLists.txt new file mode 100644 index 0000000000..7d89ba1e40 --- /dev/null +++ b/libraries/AP_Common/CMakeLists.txt @@ -0,0 +1,33 @@ +set(LIB_NAME AP_Common) + +set(${LIB_NAME}_SRCS + AP_Common.cpp + AP_Loop.cpp + AP_MetaClass.cpp + AP_Var.cpp + AP_Var_menufuncs.cpp + c++.cpp + menu.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Common.h + AP_Loop.h + AP_MetaClass.h + AP_Var.h + AP_Test.h + c++.h + AP_Vector.h +) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Compass/CMakeLists.txt b/libraries/AP_Compass/CMakeLists.txt new file mode 100644 index 0000000000..613b751ef4 --- /dev/null +++ b/libraries/AP_Compass/CMakeLists.txt @@ -0,0 +1,27 @@ +set(LIB_NAME AP_Compass) + +set(${LIB_NAME}_SRCS + AP_Compass_HIL.cpp + AP_Compass_HMC5843.cpp + Compass.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + Compass.h + AP_Compass_HIL.h + AP_Compass_HMC5843.h + AP_Compass.h + ) + + + +include_directories( + + - + ${ARDUINO_LIBRARIES_PATH}/Wire + #${CMAKE_SOURCE_DIR}/libraries/FastSerial + # +) +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_DCM/CMakeLists.txt b/libraries/AP_DCM/CMakeLists.txt new file mode 100644 index 0000000000..0131eeb495 --- /dev/null +++ b/libraries/AP_DCM/CMakeLists.txt @@ -0,0 +1,25 @@ +set(LIB_NAME AP_DCM) + +set(${LIB_NAME}_SRCS + AP_DCM.cpp + AP_DCM_HIL.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_DCM.h + AP_DCM_HIL.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_DCM + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_GPS/CMakeLists.txt b/libraries/AP_GPS/CMakeLists.txt new file mode 100644 index 0000000000..57409c6d24 --- /dev/null +++ b/libraries/AP_GPS/CMakeLists.txt @@ -0,0 +1,44 @@ +set(LIB_NAME AP_GPS) + +set(${LIB_NAME}_SRCS + AP_GPS_406.cpp + AP_GPS_Auto.cpp + AP_GPS_HIL.cpp + AP_GPS_IMU.cpp + AP_GPS_MTK.cpp + AP_GPS_MTK16.cpp + AP_GPS_NMEA.cpp + AP_GPS_SIRF.cpp + AP_GPS_UBLOX.cpp + GPS.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_GPS_406.h + AP_GPS_Auto.h + AP_GPS_HIL.h + AP_GPS_IMU.h + AP_GPS_MTK.h + AP_GPS_MTK_Common.h + AP_GPS_MTK16.h + AP_GPS_NMEA.h + AP_GPS_None.h + AP_GPS_Shim.h + AP_GPS_SIRF.h + AP_GPS_UBLOX.h + AP_GPS.h + GPS.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_IMU/CMakeLists.txt b/libraries/AP_IMU/CMakeLists.txt new file mode 100644 index 0000000000..2447a078c4 --- /dev/null +++ b/libraries/AP_IMU/CMakeLists.txt @@ -0,0 +1,25 @@ +set(LIB_NAME AP_IMU) + +set(${LIB_NAME}_SRCS + AP_IMU_Oilpan.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_IMU.h + AP_IMU_Shim.h + AP_IMU_Oilpan.h + IMU.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Math/CMakeLists.txt b/libraries/AP_Math/CMakeLists.txt new file mode 100644 index 0000000000..f4a83b3102 --- /dev/null +++ b/libraries/AP_Math/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_Math) + +set(${LIB_NAME}_SRCS + + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Math.h + matrix3.h + vector2.h + vector3.h + ) + +include_directories( + + - +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_OpticalFlow/CMakeLists.txt b/libraries/AP_OpticalFlow/CMakeLists.txt new file mode 100644 index 0000000000..74d7457ac4 --- /dev/null +++ b/libraries/AP_OpticalFlow/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_OpticalFlow) + +set(${LIB_NAME}_SRCS + AP_OpticalFlow.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_OpticalFlow.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_PID/CMakeLists.txt b/libraries/AP_PID/CMakeLists.txt new file mode 100644 index 0000000000..14e9782e51 --- /dev/null +++ b/libraries/AP_PID/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME AP_PID) + +set(${LIB_NAME}_SRCS + AP_PID.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_PID.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_RangeFinder/CMakeLists.txt b/libraries/AP_RangeFinder/CMakeLists.txt new file mode 100644 index 0000000000..add2e2928e --- /dev/null +++ b/libraries/AP_RangeFinder/CMakeLists.txt @@ -0,0 +1,27 @@ +set(LIB_NAME AP_RangeFinder) + +set(${LIB_NAME}_SRCS + AP_RangeFinder_MaxsonarXL.cpp + AP_RangeFinder_SharpGP2Y.cpp + RangeFinder.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_RangeFinder.h + AP_RangeFinder_MaxsonarXL.h + AP_RangeFinder_SharpGP2Y.h + RangeFinder.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt new file mode 100644 index 0000000000..ed27448a23 --- /dev/null +++ b/libraries/CMakeLists.txt @@ -0,0 +1,24 @@ +add_subdirectory(DataFlash) +add_subdirectory(AP_Math) +add_subdirectory(PID) +add_subdirectory(AP_Common) +add_subdirectory(RC_Channel) +add_subdirectory(AP_OpticalFlow) +add_subdirectory(ModeFilter) +add_subdirectory(memcheck) +add_subdirectory(APM_PI) +add_subdirectory(AP_GPS) +add_subdirectory(AP_DCM) +add_subdirectory(AP_Compass) +add_subdirectory(AP_ADC) +add_subdirectory(AP_IMU) +add_subdirectory(AP_RangeFinder) + +add_subdirectory(APM_RC) +add_subdirectory(APM_BMP085) + +#add_subdirectory(APO) +add_subdirectory(FastSerial) +add_subdirectory(GCS_MAVLink) + +#add_subdirectory(playgroundlib) diff --git a/libraries/DataFlash/CMakeLists.txt b/libraries/DataFlash/CMakeLists.txt new file mode 100644 index 0000000000..2e13294932 --- /dev/null +++ b/libraries/DataFlash/CMakeLists.txt @@ -0,0 +1,21 @@ +set(LIB_NAME DataFlash) + +set(${LIB_NAME}_SRCS + DataFlash.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + DataFlash.h + ) + +include_directories( + + - +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/FastSerial/CMakeLists.txt b/libraries/FastSerial/CMakeLists.txt new file mode 100644 index 0000000000..78e94e9023 --- /dev/null +++ b/libraries/FastSerial/CMakeLists.txt @@ -0,0 +1,28 @@ +set(LIB_NAME FastSerial) + +set(${LIB_NAME}_SRCS + BetterStream.cpp + FastSerial.cpp + vprintf.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + BetterStream.h + FastSerial.h + ftoa_engine.h + ntz.h + xtoa_fast.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/GCS_MAVLink/CMakeLists.txt b/libraries/GCS_MAVLink/CMakeLists.txt new file mode 100644 index 0000000000..326d2b959c --- /dev/null +++ b/libraries/GCS_MAVLink/CMakeLists.txt @@ -0,0 +1,19 @@ +set(LIB_NAME GCS_MAVLink) + +set(${LIB_NAME}_SRCS + GCS_MAVLink.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + GCS_MAVLink.h +) + +include_directories( + #${CMAKE_SOURCE_DIR}/libraries/ + ${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/ModeFilter/CMakeLists.txt b/libraries/ModeFilter/CMakeLists.txt new file mode 100644 index 0000000000..cba4eec1b8 --- /dev/null +++ b/libraries/ModeFilter/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME ModeFilter) + +set(${LIB_NAME}_SRCS + ModeFilter.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + ModeFilter.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + #${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/PID/CMakeLists.txt b/libraries/PID/CMakeLists.txt new file mode 100644 index 0000000000..c814e841d1 --- /dev/null +++ b/libraries/PID/CMakeLists.txt @@ -0,0 +1,21 @@ +set(LIB_NAME PID) + +set(${LIB_NAME}_SRCS + PID.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + PID.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/RC_Channel/CMakeLists.txt b/libraries/RC_Channel/CMakeLists.txt new file mode 100644 index 0000000000..94ed383fcb --- /dev/null +++ b/libraries/RC_Channel/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME RC_Channel) + +set(${LIB_NAME}_SRCS + RC_Channel.cpp + RC_Channel_aux.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + RC_Channel.h + RC_Channel_aux.h + ) + +include_directories( + + - + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/APM_RC + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/memcheck/CMakeLists.txt b/libraries/memcheck/CMakeLists.txt new file mode 100644 index 0000000000..ec9121b1ce --- /dev/null +++ b/libraries/memcheck/CMakeLists.txt @@ -0,0 +1,24 @@ +set(LIB_NAME memcheck) + +set(${LIB_NAME}_SRCS + memcheck.cpp + #AP_OpticalFlow_ADNS3080.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + memcheck.h + #AP_OpticalFlow_ADNS3080.h + ) + +include_directories( + + - + #${CMAKE_SOURCE_DIR}/libraries/AP_Math + ${CMAKE_SOURCE_DIR}/libraries/AP_Common + #${CMAKE_SOURCE_DIR}/libraries/FastSerial +# + ) + +set(${LIB_NAME}_BOARD mega2560) + +generate_arduino_library(${LIB_NAME}) From ae81a758dec1b3b9fc23c1d27f01ae0d848cf63f Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 14 Sep 2011 13:58:18 -0700 Subject: [PATCH 053/100] Removed Simple Mode Added Simple option to mode switch positions removed unused OptFlow code added reset lat and lon error removed unused init_auto Fixed log formatting Added simple mode bitmask removed pitchmax added CLI setup for enabling Simple mode to any switch position --- ArduCopter/APM_Config.h | 9 +- ArduCopter/ArduCopter.pde | 237 ++++++++++++---------------------- ArduCopter/Attitude.pde | 31 +++-- ArduCopter/Log.pde | 119 +++++++++-------- ArduCopter/Parameters.h | 11 +- ArduCopter/commands.pde | 13 -- ArduCopter/commands_logic.pde | 16 +-- ArduCopter/config.h | 2 +- ArduCopter/control_modes.pde | 10 ++ ArduCopter/defines.h | 23 ++-- ArduCopter/setup.pde | 67 +++++----- ArduCopter/system.pde | 29 +---- 12 files changed, 240 insertions(+), 327 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 03bf89b01e..ee6dc1a740 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -46,10 +46,17 @@ CH6_TRAVERSE_SPEED */ +# define CH7_OPTION DO_SET_HOVER + /* + DO_SET_HOVER + DO_FLIP + SIMPLE_MODE_CONTROL + */ + // See the config.h and defines.h files for how to set this up! // // lets use SIMPLE mode for Roll and Pitch during Alt Hold -#define ALT_HOLD_RP ROLL_PITCH_SIMPLE +//#define ALT_HOLD_RP ROLL_PITCH_SIMPLE // lets use Manual throttle during Loiter //#define LOITER_THR THROTTLE_MANUAL diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6ad4e24031..6f38cd40ab 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -237,7 +237,6 @@ static const char *comma = ","; static const char* flight_mode_strings[] = { "STABILIZE", "ACRO", - "SIMPLE", "ALT_HOLD", "AUTO", "GUIDED", @@ -275,6 +274,7 @@ static byte control_mode = STABILIZE; static byte old_control_mode = STABILIZE; static byte oldSwitchPosition; // for remembering the control mode switch static int motor_out[8]; +static bool do_simple = false; // Heli // ---- @@ -338,7 +338,7 @@ static float cos_pitch_x = 1; static float cos_yaw_x = 1; static float sin_pitch_y, sin_yaw_y, sin_roll_y; static long initial_simple_bearing; // used for Simple mode - +static float boost; // used to give a little extra to maintain altitude // Acro #if CH7_OPTION == DO_FLIP @@ -493,7 +493,7 @@ static int gps_fix_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; +//static int mainLoop_count; static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops @@ -504,7 +504,6 @@ static uint8_t delta_ms_fiftyhz; static byte slow_loopCounter; static int superslow_loopCounter; -static byte alt_timer; // for limiting the execution of flight mode thingys static byte simple_timer; // for limiting the execution of flight mode thingys @@ -533,12 +532,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; G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator - mainLoop_count++; - - //if (delta_ms_fast_loop > 6) - // Log_Write_Performance(); // Execute the fast loop // --------------------- @@ -568,7 +562,7 @@ void loop() } if (millis() - perf_mon_timer > 20000) { - if (mainLoop_count != 0) { + //if (mainLoop_count != 0) { gcs.send_message(MSG_PERF_REPORT); @@ -576,7 +570,7 @@ void loop() Log_Write_Performance(); resetPerfData(); - } + //} } //PORTK &= B10111111; } @@ -610,7 +604,6 @@ static void fast_loop() // --------------------------------------- update_yaw_mode(); update_roll_pitch_mode(); - update_throttle_mode(); // write out the servo PWM values // ------------------------------ @@ -630,6 +623,12 @@ static void medium_loop() medium_loopCounter++; + /* + if(g.optflow_enabled){ + optflow.update() + } + */ + if(GPS_enabled){ update_GPS(); } @@ -646,6 +645,10 @@ static void medium_loop() // auto_trim, uses an auto_level algorithm auto_trim(); + + // record throttle output + // ------------------------------ + throttle_integrator += g.rc_3.servo_out; break; // This case performs some navigation computations @@ -654,13 +657,6 @@ static void medium_loop() loop_step = 2; medium_loopCounter++; - // hack to stop navigation in Simple mode - if (control_mode == SIMPLE){ - // clear GPS data - g_gps->new_data = false; - break; - } - // Auto control modes: if(g_gps->new_data && g_gps->fix){ loop_step = 11; @@ -696,17 +692,14 @@ static void medium_loop() // -------------------------- update_altitude(); - // altitude smoothing - // ------------------ - //calc_altitude_smoothing_error(); - - altitude_error = get_altitude_error(); - - //camera_stabilization(); - // invalidate the throttle hold value // ---------------------------------- invalid_throttle = true; + + // calc boost + // ---------- + boost = get_angle_boost(); + break; // This case deals with sending high rate telemetry @@ -722,11 +715,13 @@ static void medium_loop() } #if HIL_MODE != HIL_MODE_ATTITUDE - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) - Log_Write_Attitude(); + if(motor_armed){ + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) + Log_Write_Attitude(); - if (g.log_bitmask & MASK_LOG_CTUN && motor_armed) - Log_Write_Control_Tuning(); + if (g.log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); + } #endif #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK @@ -785,9 +780,9 @@ static void medium_loop() // --------------------------- static void fifty_hz_loop() { - // record throttle output - // ------------------------------ - throttle_integrator += g.rc_3.servo_out; + // moved to slower loop + // -------------------- + update_throttle_mode(); // Read Sonar // ---------- @@ -1010,86 +1005,6 @@ static void update_GPS(void) } } -#ifdef OPTFLOW_ENABLED -// blend gps and optical flow location -void update_location(void) -{ - //static int count = 0; - // get GPS position - if(GPS_enabled){ - update_GPS(); - } - - if( g.optflow_enabled ) { - int32_t temp_lat, temp_lng, diff_lat, diff_lng; - - // get optical flow position - optflow.read(); - optflow.get_position(dcm.roll, dcm.pitch, dcm.yaw, current_loc.alt-home.alt); - - // write to log - if (g.log_bitmask & MASK_LOG_OPTFLOW){ - Log_Write_Optflow(); - } - - temp_lat = optflow_offset.lat + optflow.lat; - temp_lng = optflow_offset.lng + optflow.lng; - - // if we have good GPS values, don't let optical flow position stray too far - if( GPS_enabled && g_gps->fix ) { - // ensure current location is within 3m of gps location - diff_lat = g_gps->latitude - temp_lat; - diff_lng = g_gps->longitude - temp_lng; - if( diff_lat > 300 ) { - optflow_offset.lat += diff_lat - 300; - //Serial.println("lat inc!"); - } - if( diff_lat < -300 ) { - optflow_offset.lat += diff_lat + 300; - //Serial.println("lat dec!"); - } - if( diff_lng > 300 ) { - optflow_offset.lng += diff_lng - 300; - //Serial.println("lng inc!"); - } - if( diff_lng < -300 ) { - optflow_offset.lng += diff_lng + 300; - //Serial.println("lng dec!"); - } - } - - // update the current position - current_loc.lat = optflow_offset.lat + optflow.lat; - current_loc.lng = optflow_offset.lng + optflow.lng; - - /*count++; - if( count >= 20 ) { - count = 0; - Serial.println(); - Serial.print("lat:"); - Serial.print(current_loc.lat); - Serial.print("\tlng:"); - Serial.print(current_loc.lng); - Serial.print("\tr:"); - Serial.print(nav_roll); - Serial.print("\tp:"); - Serial.print(nav_pitch); - Serial.println(); - }*/ - - // indicate we have a new position for nav functions - new_location = true; - - }else{ - // get current position from gps - current_loc.lng = g_gps->longitude; // Lon * 10 * *7 - current_loc.lat = g_gps->latitude; // Lat * 10 * *7 - - new_location = g_gps->new_data; - } -} -#endif - void update_yaw_mode(void) { switch(yaw_mode){ @@ -1100,8 +1015,11 @@ void update_yaw_mode(void) case YAW_HOLD: // calcualte new nav_yaw offset - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); - //Serial.printf("n:%d %ld, ",g.rc_4.control_in, nav_yaw); + if (control_mode <= STABILIZE){ + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); + }else{ + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); + } break; case YAW_LOOK_AT_HOME: @@ -1136,6 +1054,29 @@ void update_roll_pitch_mode(void) int control_roll = 0, control_pitch = 0; + if(do_simple){ + simple_timer++; + if(simple_timer > 5){ + simple_timer = 0; + + int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + + // pre-calc rotation (stored in real degrees) + // roll + float cos_x = sin(radians(90 - delta)); + // pitch + float sin_y = cos(radians(90 - delta)); + + // Rotate input by the initial bearing + // roll + control_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y; + // pitch + control_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x); + + g.rc_1.control_in = control_roll; + g.rc_2.control_in = control_pitch; + } + } switch(roll_pitch_mode){ case ROLL_PITCH_ACRO: @@ -1152,29 +1093,6 @@ void update_roll_pitch_mode(void) control_pitch = g.rc_2.control_in; break; - case ROLL_PITCH_SIMPLE: - simple_timer++; - if(simple_timer > 5){ - simple_timer = 0; - - int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; - - // pre-calc rotation (stored in real degrees) - // roll - float cos_x = sin(radians(90 - delta)); - // pitch - float sin_y = cos(radians(90 - delta)); - - // Rotate input by the initial bearing - // roll - nav_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y; - // pitch - nav_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x); - } - control_roll = nav_roll; - control_pitch = nav_pitch; - break; - case ROLL_PITCH_AUTO: // mix in user control with Nav control control_roll = g.rc_1.control_mix(nav_roll); @@ -1194,7 +1112,7 @@ void update_throttle_mode(void) { switch(throttle_mode){ case THROTTLE_MANUAL: - g.rc_3.servo_out = get_throttle(g.rc_3.control_in); + g.rc_3.servo_out = g.rc_3.control_in + boost; break; case THROTTLE_HOLD: @@ -1204,16 +1122,20 @@ void update_throttle_mode(void) // fall through case THROTTLE_AUTO: + // 10hz, don't run up i term if(invalid_throttle && motor_auto_armed == true){ + // how far off are we + altitude_error = get_altitude_error(); + // get the AP throttle nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s - // apply throttle control - g.rc_3.servo_out = get_throttle(nav_throttle); - // clear the new data flag invalid_throttle = false; } + + // apply throttle control at 200 hz + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + boost; break; } } @@ -1237,6 +1159,11 @@ static void update_navigation() break; case GUIDED: + wp_control = WP_MODE; + update_auto_yaw(); + update_nav_wp(); + break; + case RTL: if(wp_distance > 4){ // calculates desired Yaw @@ -1251,7 +1178,6 @@ static void update_navigation() //xtrack_enabled = false; } - // calculates the desired Roll and Pitch update_nav_wp(); break; @@ -1273,8 +1199,8 @@ static void update_navigation() //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)); + target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(radians(90 - circle_angle))); + target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(radians(90 - circle_angle))); } // calc the lat and long error to the target @@ -1379,18 +1305,13 @@ static void update_altitude() static void adjust_altitude() { - alt_timer++; - if(alt_timer >= 2){ - alt_timer = 0; - - if(g.rc_3.control_in <= 200){ - next_WP.alt -= 1; // 1 meter per second - next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location - next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter - }else if (g.rc_3.control_in > 700){ - next_WP.alt += 1; // 1 meter per second - next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location - } + if(g.rc_3.control_in <= 200){ + next_WP.alt -= 1; // 1 meter per second + next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location + next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter + }else if (g.rc_3.control_in > 700){ + next_WP.alt += 1; // 1 meter per second + next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 8a9512664d..b6672e0752 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -91,7 +91,7 @@ static int get_nav_throttle(long z_error, int target_speed) { int rate_error; - int throttle; + //int throttle; float scaler = (float)target_speed/(float)ALT_ERROR_MAX; // limit error to prevent I term run up @@ -101,8 +101,11 @@ get_nav_throttle(long z_error, int target_speed) rate_error = target_speed - altitude_rate; rate_error = constrain(rate_error, -110, 110); - throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); - return g.throttle_cruise + throttle; + //throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); + //return g.throttle_cruise + throttle; + + + return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); } @@ -156,10 +159,16 @@ static void reset_hold_I(void) // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -static void reset_nav_I(void) +static void reset_nav(void) { + nav_throttle = 0; + invalid_throttle = true; + g.pi_nav_lat.reset_I(); g.pi_nav_lon.reset_I(); + + long_error = 0; + lat_error = 0; } @@ -169,11 +178,11 @@ throttle control // user input: // ----------- -static int get_throttle(int throttle_input) -{ - throttle_input = (float)throttle_input * angle_boost(); - return max(throttle_input, 0); -} +//static int get_throttle(int throttle_input) +//{ +// throttle_input = (float)throttle_input * angle_boost(); +// return max(throttle_input, 0); +//} static long get_nav_yaw_offset(int yaw_input, int reset) @@ -188,7 +197,7 @@ get_nav_yaw_offset(int yaw_input, int reset) // re-define nav_yaw if we have stick input if(yaw_input != 0){ // set nav_yaw + or - the current location - _yaw = (long)yaw_input + dcm.yaw_sensor; + _yaw = (long)yaw_input + dcm.yaw_sensor; // we need to wrap our value so we can be 0 to 360 (*100) return wrap_360(_yaw); @@ -210,7 +219,7 @@ static int alt_hold_velocity() } */ -static float angle_boost() +static float get_angle_boost() { float temp = cos_pitch_x * cos_roll_x; temp = 2.0 - constrain(temp, .5, 1.0); diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 9f1ef3a578..f96b3ffe2c 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -365,16 +365,15 @@ static void Log_Write_GPS() DataFlash.WriteByte(LOG_GPS_MSG); DataFlash.WriteLong(g_gps->time); // 1 - DataFlash.WriteByte(g_gps->fix); // 2 - DataFlash.WriteByte(g_gps->num_sats); // 3 + DataFlash.WriteByte(g_gps->num_sats); // 2 - DataFlash.WriteLong(current_loc.lat); // 4 - DataFlash.WriteLong(current_loc.lng); // 5 - DataFlash.WriteLong(current_loc.alt); // 7 + DataFlash.WriteLong(current_loc.lat); // 3 + DataFlash.WriteLong(current_loc.lng); // 4 + DataFlash.WriteLong(current_loc.alt); // 5 DataFlash.WriteLong(g_gps->altitude); // 6 - DataFlash.WriteInt(g_gps->ground_speed); // 8 - DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 9 + DataFlash.WriteInt(g_gps->ground_speed); // 7 + DataFlash.WriteInt((uint16_t)g_gps->ground_course); // 8 DataFlash.WriteByte(END_BYTE); } @@ -382,21 +381,20 @@ static void Log_Write_GPS() // Read a GPS packet static void Log_Read_GPS() { - Serial.printf_P(PSTR("GPS, %ld, %d, %d, " + Serial.printf_P(PSTR("GPS, %ld, %d, " "%4.7f, %4.7f, %4.4f, %4.4f, " "%d, %u\n"), DataFlash.ReadLong(), // 1 time - (int)DataFlash.ReadByte(), // 2 fix - (int)DataFlash.ReadByte(), // 3 sats + (int)DataFlash.ReadByte(), // 2 sats - (float)DataFlash.ReadLong() / t7, // 4 lat - (float)DataFlash.ReadLong() / t7, // 5 lon - (float)DataFlash.ReadLong() / 100.0, // 6 gps alt - (float)DataFlash.ReadLong() / 100.0, // 7 sensor alt + (float)DataFlash.ReadLong() / t7, // 3 lat + (float)DataFlash.ReadLong() / t7, // 4 lon + (float)DataFlash.ReadLong() / 100.0, // 5 gps alt + (float)DataFlash.ReadLong() / 100.0, // 6 sensor alt - DataFlash.ReadInt(), // 8 ground speed - (uint16_t)DataFlash.ReadInt()); // 9 ground course + DataFlash.ReadInt(), // 7 ground speed + (uint16_t)DataFlash.ReadInt()); // 8 ground course } @@ -514,7 +512,7 @@ static void Log_Write_Optflow() } #endif -// Read an attitude packet + static void Log_Read_Optflow() { Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), @@ -609,23 +607,18 @@ static void Log_Write_Control_Tuning() DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); - // Control - //DataFlash.WriteInt((int)(g.rc_4.control_in/100)); - //DataFlash.WriteInt((int)(g.rc_4.servo_out/100)); - // yaw - DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); - DataFlash.WriteInt((int)(nav_yaw/100)); - DataFlash.WriteInt((int)yaw_error/100); + DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1 + DataFlash.WriteInt((int)(nav_yaw/100)); //2 + DataFlash.WriteInt((int)yaw_error/100); //3 // Alt hold - DataFlash.WriteInt(g.rc_3.servo_out); - DataFlash.WriteInt(sonar_alt); // - DataFlash.WriteInt(baro_alt); // + DataFlash.WriteInt(g.rc_3.servo_out); //4 + DataFlash.WriteInt(sonar_alt); //5 + DataFlash.WriteInt(baro_alt); //6 - DataFlash.WriteInt((int)next_WP.alt); // - //DataFlash.WriteInt((int)altitude_error); // - DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); + DataFlash.WriteInt((int)next_WP.alt); //7 + DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8 DataFlash.WriteByte(END_BYTE); } @@ -635,10 +628,9 @@ static void Log_Write_Control_Tuning() static void Log_Read_Control_Tuning() { Serial.printf_P(PSTR( "CTUN, " - //"%d, %d, " "%d, %d, %d, " "%d, %d, %d, " - "%d, %d, %d\n"), + "%d, %d\n"), // Control DataFlash.ReadInt(), @@ -670,21 +662,18 @@ static void Log_Write_Performance() //* - DataFlash.WriteLong( millis()- perf_mon_timer); - DataFlash.WriteInt ( mainLoop_count); - DataFlash.WriteInt ( G_Dt_max); + //DataFlash.WriteLong( millis()- perf_mon_timer); + //DataFlash.WriteInt ( mainLoop_count); + DataFlash.WriteInt ( G_Dt_max); //1 - DataFlash.WriteByte( dcm.gyro_sat_count); - DataFlash.WriteByte( imu.adc_constraints); - DataFlash.WriteByte( dcm.renorm_sqrt_count); - DataFlash.WriteByte( dcm.renorm_blowup_count); - DataFlash.WriteByte( gps_fix_count); + DataFlash.WriteByte( dcm.gyro_sat_count); //2 + DataFlash.WriteByte( imu.adc_constraints); //3 + DataFlash.WriteByte( dcm.renorm_sqrt_count); //4 + DataFlash.WriteByte( dcm.renorm_blowup_count); //5 + DataFlash.WriteByte( gps_fix_count); //6 - DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); - DataFlash.WriteLong ( throttle_integrator); - - //*/ - //PM, 20005, 3742, 10,0,0,0,0,89,1000, + DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //7 + DataFlash.WriteLong ( throttle_integrator); //8 DataFlash.WriteByte(END_BYTE); } @@ -692,24 +681,23 @@ static void Log_Write_Performance() // Read a performance packet static void Log_Read_Performance() { - Serial.printf_P(PSTR( "PM, %ld, %d, %d, " - "%d, %d, %d, %d, %d, " + Serial.printf_P(PSTR( "PM, %d, %d, %d, " + "%d, %d, %d, " "%d, %ld\n"), // Control - DataFlash.ReadLong(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), + //DataFlash.ReadLong(), + //DataFlash.ReadInt(), + DataFlash.ReadInt(), //1 + DataFlash.ReadByte(), //2 + DataFlash.ReadByte(), //3 - DataFlash.ReadByte(), - DataFlash.ReadByte(), + DataFlash.ReadByte(), //4 + DataFlash.ReadByte(), //5 + DataFlash.ReadByte(), //6 - DataFlash.ReadByte(), - DataFlash.ReadByte(), - DataFlash.ReadByte(), - - DataFlash.ReadInt(), - DataFlash.ReadLong()); + DataFlash.ReadInt(), //7 + DataFlash.ReadLong()); //8 } // Write a command processing packet. @@ -760,19 +748,28 @@ static void Log_Write_Attitude() DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_ATTITUDE_MSG); + DataFlash.WriteInt((int)dcm.roll_sensor); DataFlash.WriteInt((int)dcm.pitch_sensor); DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); + + DataFlash.WriteInt((int)g.rc_1.servo_out); + DataFlash.WriteInt((int)g.rc_2.servo_out); + DataFlash.WriteInt((int)g.rc_4.servo_out); + DataFlash.WriteByte(END_BYTE); } // Read an attitude packet static void Log_Read_Attitude() { - Serial.printf_P(PSTR("ATT, %d, %d, %u\n"), + Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, $d\n"), DataFlash.ReadInt(), DataFlash.ReadInt(), - (uint16_t)DataFlash.ReadInt()); + (uint16_t)DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt()); } // Write a mode packet. Total length : 5 bytes @@ -827,6 +824,8 @@ static void Log_Read(int start_page, int end_page) case 0: if(data == HEAD_BYTE1) // Head byte 1 log_step++; + else + Serial.println("."); break; case 1: diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c2680f856e..c6e6d7cc67 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -83,7 +83,6 @@ public: // 160: Navigation parameters // k_param_crosstrack_entry_angle = 160, - k_param_pitch_max, k_param_RTL_altitude, // @@ -139,7 +138,7 @@ public: k_param_flight_mode4, k_param_flight_mode5, k_param_flight_mode6, - + k_param_simple_modes, // // 220: Waypoint data @@ -179,7 +178,7 @@ public: // AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; - AP_Int8 serial3_baud; + AP_Int8 serial3_baud; // Crosstrack navigation @@ -213,6 +212,7 @@ public: AP_Int8 flight_mode4; AP_Int8 flight_mode5; AP_Int8 flight_mode6; + AP_Int8 simple_modes; // Radio settings // @@ -221,8 +221,6 @@ public: //AP_Var_group pwm_throttle; //AP_Var_group pwm_yaw; - AP_Int16 pitch_max; - // Misc // AP_Int16 log_bitmask; @@ -324,8 +322,7 @@ public: 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")), - - pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")), + simple_modes (0, k_param_simple_modes, PSTR("SIMPLE")), 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")), diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index ec1da8f3ff..ff7e6d6bb7 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -1,18 +1,5 @@ // -*- 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. diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 68dbce1c55..b9581a7705 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -311,13 +311,10 @@ static void do_loiter_turns() static void do_loiter_time() { - ///* - wp_control = LOITER_MODE; + wp_control = LOITER_MODE; set_next_WP(¤t_loc); loiter_time = millis(); loiter_time_max = next_command.p1 * 1000; // units are (seconds) - //Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max); - //*/ } /********************************************************************************/ @@ -464,11 +461,12 @@ static void do_change_alt() { Location temp = next_WP; condition_start = current_loc.alt; - if (next_command.options & WP_OPTION_ALT_RELATIVE) { - condition_value = next_command.alt + home.alt; - } else { - condition_value = next_command.alt; - } + condition_value = next_command.alt; + //if (next_command.options & WP_OPTION_ALT_RELATIVE) { + // condition_value = next_command.alt + home.alt; + //} else { + + //} temp.alt = condition_value; set_next_WP(&temp); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 144c8f650d..2f2fe769c4 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -579,7 +579,7 @@ # define LOG_ATTITUDE_FAST DISABLED #endif #ifndef LOG_ATTITUDE_MED -# define LOG_ATTITUDE_MED DISABLED +# define LOG_ATTITUDE_MED ENABLED #endif #ifndef LOG_GPS # define LOG_GPS ENABLED diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 088f5e3288..a12957ac97 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -13,6 +13,12 @@ static void read_control_switch() switch_debouncer = false; set_mode(flight_modes[switchPosition]); + + // setup Simple mode + // do we enable simple mode? + do_simple = (g.simple_modes & 1 << switchPosition); + Serial.printf("do simple: %d \n", (int)do_simple); + }else{ switch_debouncer = true; } @@ -47,6 +53,10 @@ static void read_trim_switch() do_flip = true; } +#elif CH7_OPTION == SIMPLE_MODE_CONTROL + + do_simple = (g.rc_7.control_in > 800); + #elif CH7_OPTION == DO_SET_HOVER // switch is engaged if (g.rc_7.control_in > 800){ diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 9bd0fb9f88..1bbd29cbcf 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -30,6 +30,7 @@ // CH 7 control #define DO_SET_HOVER 0 #define DO_FLIP 1 +#define SIMPLE_MODE_CONTROL 2 // Frame types #define QUAD_FRAME 0 @@ -121,14 +122,20 @@ // ---------------- #define STABILIZE 0 // hold level position #define ACRO 1 // rate control -#define SIMPLE 2 // -#define ALT_HOLD 3 // AUTO control -#define AUTO 4 // AUTO control -#define GUIDED 5 // AUTO control -#define LOITER 6 // Hold a single location -#define RTL 7 // AUTO control -#define CIRCLE 8 // AUTO control -#define NUM_MODES 9 +#define ALT_HOLD 2 // AUTO control +#define AUTO 3 // AUTO control +#define GUIDED 4 // AUTO control +#define LOITER 5 // Hold a single location +#define RTL 6 // AUTO control +#define CIRCLE 7 // AUTO control +#define NUM_MODES 8 + +#define SIMPLE_1 1 +#define SIMPLE_2 2 +#define SIMPLE_3 4 +#define SIMPLE_4 8 +#define SIMPLE_5 16 +#define SIMPLE_6 32 // CH_6 Tuning // ----------- diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 529297aa2d..552fbee763 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -101,9 +101,11 @@ setup_show(uint8_t argc, const Menu::arg *argv) report_flight_modes(); report_imu(); report_compass(); + #ifdef OPTFLOW_ENABLED report_optflow(); #endif + #if FRAME_CONFIG == HELI_FRAME report_heli(); report_gyro(); @@ -302,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) byte _oldSwitchPosition = 0; byte mode = 0; - Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); + Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off")); print_hit_enter(); while(1){ @@ -318,14 +320,14 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode = constrain(mode, 0, NUM_MODES-1); // update the user - print_switch(_switchPosition, mode); + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); // Remember switch position _oldSwitchPosition = _switchPosition; } // look for stick input - if (radio_input_switch() == true){ + if (abs(g.rc_1.control_in) > 3000){ mode++; if(mode >= NUM_MODES) mode = 0; @@ -334,13 +336,31 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) flight_modes[_switchPosition] = mode; // print new mode - print_switch(_switchPosition, mode); + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } + + // look for stick input + if (abs(g.rc_4.control_in) > 3000){ + g.simple_modes |= (1<<_switchPosition); + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); + } + + // look for stick input + if (abs(g.rc_4.control_in) < 3000){ + g.simple_modes &= ~(1<<_switchPosition); + // print new mode + print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); + delay(500); } // escape hatch if(Serial.available() > 0){ - for (mode=0; mode<6; mode++) + for (mode = 0; mode < 6; mode++) flight_modes[mode].save(); + print_done(); report_flight_modes(); return (0); @@ -872,7 +892,7 @@ static void report_flight_modes() print_divider(); for(int i = 0; i < 6; i++ ){ - print_switch(i, flight_modes[i]); + print_switch(i, flight_modes[i], (g.simple_modes & (1< 100) { - bouncer = 10; - } - if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) < -100) { - bouncer = -10; - } - if (bouncer >0) { - bouncer --; - } - if (bouncer <0) { - bouncer ++; - } - - if (bouncer == 1 || bouncer == -1) { - return bouncer; - }else{ - return 0; - } -} - static void zero_eeprom(void) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 6eb3c83e8f..55f8cec2be 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -249,11 +249,6 @@ static void init_ardupilot() start_new_log(); } - //#if(GROUND_START_DELAY > 0) - //gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay")); - // delay(GROUND_START_DELAY * 1000); - //#endif - GPS_enabled = false; // Read in the GPS @@ -292,10 +287,8 @@ static void init_ardupilot() // --------------------------- reset_control_switch(); - //delay(100); startup_ground(); - //Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max); Log_Write_Startup(); SendDebug("\nReady to FLY "); @@ -361,9 +354,7 @@ static void set_mode(byte mode) // report the GPS and Motor arming status led_mode = NORMAL_LEDS; - // most modes do not calculate crosstrack correction - //xtrack_enabled = false; - reset_nav_I(); + reset_nav(); switch(control_mode) { @@ -381,13 +372,6 @@ static void set_mode(byte mode) reset_hold_I(); break; - case SIMPLE: - yaw_mode = SIMPLE_YAW; - roll_pitch_mode = SIMPLE_RP; - throttle_mode = SIMPLE_THR; - reset_hold_I(); - break; - case ALT_HOLD: yaw_mode = ALT_HOLD_YAW; roll_pitch_mode = ALT_HOLD_RP; @@ -395,7 +379,7 @@ static void set_mode(byte mode) reset_hold_I(); init_throttle_cruise(); - next_WP.alt = current_loc.alt; + next_WP = current_loc; break; case AUTO: @@ -407,12 +391,7 @@ static void set_mode(byte mode) init_throttle_cruise(); // loads the commands from where we left off - //init_auto(); init_commands(); - - // do crosstrack correction - // XXX move to flight commands - //xtrack_enabled = true; break; case CIRCLE: @@ -491,7 +470,7 @@ static void set_failsafe(boolean mode) static void resetPerfData(void) { - mainLoop_count = 0; + //mainLoop_count = 0; G_Dt_max = 0; gps_fix_count = 0; perf_mon_timer = millis(); @@ -541,7 +520,7 @@ static void init_throttle_cruise() { // are we moving from manual throttle to auto_throttle? - if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ + if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ g.pi_throttle.reset_I(); g.throttle_cruise.set_and_save(g.rc_3.control_in); } From f79d4caeffa3d9031fdfe3b39597c25753660f0b Mon Sep 17 00:00:00 2001 From: Mike Smith Date: Wed, 14 Sep 2011 22:39:34 -0700 Subject: [PATCH 054/100] Add options to generate assembly listings for all of the compiled files. This is a possible starting point for a tool to analyse the call graph and stack usage of APM. It's also interesting by itself. --- libraries/AP_Common/Arduino.mk | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index f84f06a237..894a92388c 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -205,10 +205,11 @@ DEPFLAGS = -MD -MT $@ CXXOPTS = -mcall-prologues -ffunction-sections -fdata-sections -fno-exceptions COPTS = -mcall-prologues -ffunction-sections -fdata-sections ASOPTS = -assembler-with-cpp +LISTOPTS = -adhlns=$(@:.o=.lst) -CXXFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS) -CFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(COPTS) -ASFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(DEPFLAGS) $(ASOPTS) +CXXFLAGS = -g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS) +CFLAGS = -g -mmcu=$(MCU) $(DEFINES) -Wa,$(LISTOPTS) $(OPTFLAGS) $(DEPFLAGS) $(COPTS) +ASFLAGS = -g -mmcu=$(MCU) $(DEFINES) $(LISTOPTS) $(DEPFLAGS) $(ASOPTS) LDFLAGS = -g -mmcu=$(MCU) $(OPTFLAGS) -Wl,--gc-sections -Wl,-Map -Wl,$(SKETCHMAP) LIBS = -lm From c6eae87f8af042e8687b5685a8772b40bcf9efdb Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Thu, 15 Sep 2011 18:26:59 +0800 Subject: [PATCH 055/100] fix remote error value --- ArduCopter/GCS_Mavlink.pde | 1 + ArduPlane/GCS_Mavlink.pde | 1 + 2 files changed, 2 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 261b591986..9dd7ee4a32 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -51,6 +51,7 @@ GCS_MAVLINK::update(void) // receive new packets mavlink_message_t msg; mavlink_status_t status; + status.packet_rx_drop_count = 0; // process received bytes while(comm_get_available(chan)) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index db34c15389..b37ecb791f 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -51,6 +51,7 @@ GCS_MAVLINK::update(void) // receive new packets mavlink_message_t msg; mavlink_status_t status; + status.packet_rx_drop_count = 0; // process received bytes while(comm_get_available(chan)) From 45c4872e6de39df6c78f37588343f26f8c328809 Mon Sep 17 00:00:00 2001 From: Janne M Date: Thu, 15 Sep 2011 22:38:22 +0300 Subject: [PATCH 056/100] Issue #415: Modified spotlight search string. Added UPLOAD_PROTOCOL parameter to makefile to use different programmer. --- libraries/AP_Common/Arduino.mk | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index 894a92388c..be6928f17c 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -124,7 +124,7 @@ ifeq ($(ARDUINO),) # ifeq ($(SYSTYPE),Darwin) # use Spotlight to find Arduino.app - ARDUINO_QUERY = 'kMDItemKind == Application && kMDItemDisplayName == Arduino.app' + ARDUINO_QUERY = 'kMDItemKind == Application && kMDItemFSName == Arduino.app' ARDUINOS := $(addsuffix /Contents/Resources/Java,$(shell mdfind -literal $(ARDUINO_QUERY))) ifeq ($(ARDUINOS),) $(error ERROR: Spotlight cannot find Arduino on your system.) @@ -321,7 +321,9 @@ HARDWARE_CORE := $(shell grep $(BOARD).build.core $(BOARDFILE) | cut -d = -f 2) UPLOAD_SPEED := $(shell grep $(BOARD).upload.speed $(BOARDFILE) | cut -d = -f 2) # This simply does not work, so hardcode it to the correct value #UPLOAD_PROTOCOL := $(shell grep $(BOARD).upload.protocol $(BOARDFILE) | cut -d = -f 2) -UPLOAD_PROTOCOL := arduino +ifeq ($(UPLOAD_PROTOCOL),) + UPLOAD_PROTOCOL := arduino +endif ifeq ($(MCU),) $(error ERROR: Could not locate board $(BOARD) in $(BOARDFILE)) From 9a508a20f42381036912c17a8337e85e1739a209 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 15 Sep 2011 23:33:00 -0700 Subject: [PATCH 057/100] Integration of optical flow bug fixes for new Simple mode increased Baro filter by 2 fields --- ArduCopter/APM_Config.h | 2 - ArduCopter/ArduCopter.pde | 126 +++++++++------------- ArduCopter/Attitude.pde | 6 -- ArduCopter/Log.pde | 7 +- ArduCopter/commands_logic.pde | 7 +- ArduCopter/config.h | 27 +---- ArduCopter/control_modes.pde | 12 ++- ArduCopter/defines.h | 3 +- ArduCopter/navigation.pde | 24 ++--- ArduCopter/radio.pde | 2 +- ArduCopter/setup.pde | 19 ++-- ArduCopter/system.pde | 12 ++- ArduCopter/test.pde | 1 + libraries/APM_BMP085/APM_BMP085.h | 2 +- libraries/AP_RC_Channel/AP_RC_Channel.cpp | 10 +- libraries/AP_RC_Channel/AP_RC_Channel.h | 10 +- 16 files changed, 102 insertions(+), 168 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index ee6dc1a740..870a07cc40 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -55,8 +55,6 @@ // See the config.h and defines.h files for how to set this up! // -// lets use SIMPLE mode for Roll and Pitch during Alt Hold -//#define ALT_HOLD_RP ROLL_PITCH_SIMPLE // lets use Manual throttle during Loiter //#define LOITER_THR THROTTLE_MANUAL diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6f38cd40ab..69e92c4b7b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -61,7 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List #include // PI library #include // RC Channel Library #include // Range finder library -#include // Optical Flow library +#include // Optical Flow library #include #include // MAVLink GCS definitions #include @@ -127,9 +127,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1; APM_BMP085_Class barometer; AP_Compass_HMC5843 compass(Parameters::k_param_compass); - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; - #endif // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -171,7 +168,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1; AP_Compass_HIL compass; // never used AP_IMU_Shim imu; // never used #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; + AP_OpticalFlow_ADNS3080 optflow(&dcm); #endif static int32_t gps_base_alt; #else @@ -201,6 +198,16 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #endif // normal dcm AP_DCM dcm(&imu, g_gps); + + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow(&dcm); + #endif +#endif + +#elif HIL_MODE == HIL_MODE_ATTITUDE + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow(&dcm); + #endif #endif //////////////////////////////////////////////////////////////////////////////// @@ -467,15 +474,12 @@ static struct Location next_command; // command preloaded static struct Location guided_WP; // guided mode waypoint static long target_altitude; // used for static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location -static struct Location optflow_offset; // optical flow base position static boolean new_location; // flag to tell us if location has been updated - // IMU variables // ------------- static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) - // Performance monitoring // ---------------------- static long perf_mon_timer; @@ -483,17 +487,10 @@ static long perf_mon_timer; static int G_Dt_max; // Max main loop cycle time in milliseconds static int gps_fix_count; -// GCS -// --- -//static char GCS_buffer[53]; -//static 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; static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops @@ -514,6 +511,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS static byte counter_one_herz; static bool GPS_enabled = false; static byte loop_step; +static bool new_radio_frame; //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -537,7 +535,6 @@ void loop() // Execute the fast loop // --------------------- fast_loop(); - fast_loopTimeStamp = millis(); } //PORTK &= B11101111; @@ -562,15 +559,12 @@ void loop() } if (millis() - perf_mon_timer > 20000) { - //if (mainLoop_count != 0) { + gcs.send_message(MSG_PERF_REPORT); - gcs.send_message(MSG_PERF_REPORT); + if (g.log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); - if (g.log_bitmask & MASK_LOG_PM) - Log_Write_Performance(); - - resetPerfData(); - //} + resetPerfData(); } //PORTK &= B10111111; } @@ -623,11 +617,15 @@ static void medium_loop() medium_loopCounter++; - /* if(g.optflow_enabled){ - optflow.update() + optflow.read(); + optflow.update_position(cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + + // write to log + if (g.log_bitmask & MASK_LOG_OPTFLOW){ + Log_Write_Optflow(); + } } - */ if(GPS_enabled){ update_GPS(); @@ -795,19 +793,8 @@ static void fifty_hz_loop() hil.send_message(MSG_RADIO_OUT); #endif - // use Yaw to find our bearing error - //calc_bearing_error(); - - //if (throttle_slew < 0) - // throttle_slew++; - //else if (throttle_slew > 0) - // throttle_slew--; - camera_stabilization(); - - //throttle_slew = min((800 - g.rc_3.control_in), throttle_slew); - # if HIL_MODE == HIL_MODE_DISABLED if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude(); @@ -882,10 +869,10 @@ static void slow_loop() #if AUTO_RESET_LOITER == 1 if(control_mode == LOITER){ - if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ + //if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ // reset LOITER to current position //next_WP = current_loc; - } + //} } #endif @@ -915,11 +902,6 @@ static void slow_loop() if(g.radio_tuning > 0) tuning(); - // filter out the baro offset. - //if(baro_alt_offset > 0) baro_alt_offset--; - //if(baro_alt_offset < 0) baro_alt_offset++; - - #if MOTOR_LEDS == 1 update_motor_leds(); #endif @@ -1005,6 +987,7 @@ static void update_GPS(void) } } + void update_yaw_mode(void) { switch(yaw_mode){ @@ -1054,28 +1037,19 @@ void update_roll_pitch_mode(void) int control_roll = 0, control_pitch = 0; - if(do_simple){ - simple_timer++; - if(simple_timer > 5){ - simple_timer = 0; + //read_radio(); - int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + if(do_simple && new_radio_frame){ + new_radio_frame = false; + //Serial.printf("1: %d, 2: %d ",g.rc_1.control_in, g.rc_2.control_in); - // pre-calc rotation (stored in real degrees) - // roll - float cos_x = sin(radians(90 - delta)); - // pitch - float sin_y = cos(radians(90 - delta)); + // Rotate input by the initial bearing + control_roll = g.rc_1.control_in * sin_yaw_y + g.rc_2.control_in * cos_yaw_x; + control_pitch = g.rc_2.control_in * sin_yaw_y - g.rc_1.control_in * cos_yaw_x; - // Rotate input by the initial bearing - // roll - control_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y; - // pitch - control_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x); - - g.rc_1.control_in = control_roll; - g.rc_2.control_in = control_pitch; - } + g.rc_1.control_in = control_roll; + g.rc_2.control_in = control_pitch; + //Serial.printf("\t1: %d, 2: %d\n",g.rc_1.control_in, g.rc_2.control_in); } switch(roll_pitch_mode){ @@ -1085,29 +1059,26 @@ void update_roll_pitch_mode(void) // Pitch control g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); - return; break; case ROLL_PITCH_STABLE: - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; + g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); break; case ROLL_PITCH_AUTO: // mix in user control with Nav control - control_roll = g.rc_1.control_mix(nav_roll); - control_pitch = g.rc_2.control_mix(nav_pitch); + control_roll = g.rc_1.control_mix(nav_roll); + control_pitch = g.rc_2.control_mix(nav_pitch); + g.rc_1.servo_out = get_stabilize_roll(control_roll); + g.rc_2.servo_out = get_stabilize_pitch(control_pitch); break; } - // Roll control - g.rc_1.servo_out = get_stabilize_roll(control_roll); - - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); } +// 50 hz update rate, not 250 void update_throttle_mode(void) { switch(throttle_mode){ @@ -1238,8 +1209,6 @@ static void update_trig(void){ yawvector.y = temp.b.x; // cos yawvector.normalize(); - cos_yaw_x = yawvector.y; // 0 x = north - sin_yaw_y = yawvector.x; // 1 y sin_pitch_y = -temp.c.x; cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x)); @@ -1247,6 +1216,9 @@ static void update_trig(void){ cos_roll_x = temp.c.z / cos_pitch_x; sin_roll_y = temp.c.y / cos_pitch_x; + cos_yaw_x = yawvector.y; // 0 x = north + sin_yaw_y = yawvector.x; // 1 y + //flat: // 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00, // 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00, @@ -1366,8 +1338,8 @@ static void tuning(){ 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(); + if (g.rc_6.control_in > 525) relay_on(); + if (g.rc_6.control_in < 475) relay_off(); break; case CH6_TRAVERSE_SPEED: diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index b6672e0752..ee7b041b4b 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -91,7 +91,6 @@ static int get_nav_throttle(long z_error, int target_speed) { int rate_error; - //int throttle; float scaler = (float)target_speed/(float)ALT_ERROR_MAX; // limit error to prevent I term run up @@ -101,14 +100,9 @@ get_nav_throttle(long z_error, int target_speed) rate_error = target_speed - altitude_rate; rate_error = constrain(rate_error, -110, 110); - //throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); - //return g.throttle_cruise + throttle; - - return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); } - static int get_rate_roll(long target_rate) { diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index f96b3ffe2c..d5a71cc5a9 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -98,9 +98,6 @@ dump_log(uint8_t argc, const Menu::arg *argv) int dump_log_start; int dump_log_end; - //byte last_log_num = get_num_logs(); - //Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); - // check that the requested log number can be read dump_log = argv[1].i; @@ -506,8 +503,8 @@ static void Log_Write_Optflow() DataFlash.WriteInt((int)optflow.dx); DataFlash.WriteInt((int)optflow.dy); DataFlash.WriteInt((int)optflow.surface_quality); - DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat); - DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng); + DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); + DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); DataFlash.WriteByte(END_BYTE); } #endif diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index b9581a7705..5b3d88fe2e 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -462,12 +462,7 @@ static void do_change_alt() Location temp = next_WP; condition_start = current_loc.alt; condition_value = next_command.alt; - //if (next_command.options & WP_OPTION_ALT_RELATIVE) { - // condition_value = next_command.alt + home.alt; - //} else { - - //} - temp.alt = condition_value; + temp.alt = next_command.alt; set_next_WP(&temp); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2f2fe769c4..3f6937a374 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -207,9 +207,11 @@ ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included - //#define OPTFLOW_ENABLED + #define OPTFLOW_ENABLED #endif +#define OPTFLOW_ENABLED + #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) # define OPTFLOW DISABLED #endif @@ -310,7 +312,7 @@ #endif #ifndef SIMPLE_RP -# define SIMPLE_RP ROLL_PITCH_SIMPLE +# define SIMPLE_RP ROLL_PITCH_STABLE #endif #ifndef SIMPLE_THR @@ -413,7 +415,7 @@ // Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P .13 +# define RATE_ROLL_P 0.13 #endif #ifndef RATE_ROLL_I # define RATE_ROLL_I 0.0 @@ -488,22 +490,6 @@ # define NAV_IMAX 20 // degrees #endif -/* -#ifndef NAV_LOITER_P -# define NAV_LOITER_P .4 //1.4 // -#endif -#ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.05 // -#endif -#ifndef NAV_LOITER_D -# define NAV_LOITER_D 2 // -#endif -#ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 8 // degrees° -#endif -*/ - - #ifndef WAYPOINT_SPEED_MAX # define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph #endif @@ -518,9 +504,6 @@ #ifndef THROTTLE_I # define THROTTLE_I 0.10 // with 4m error, 12.5s windup #endif -//#ifndef THROTTLE_D -//# define THROTTLE_D 0.6 // upped with filter -//#endif #ifndef THROTTLE_IMAX # define THROTTLE_IMAX 300 #endif diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index a12957ac97..63c3918534 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -14,10 +14,13 @@ static void read_control_switch() set_mode(flight_modes[switchPosition]); - // setup Simple mode - // do we enable simple mode? - do_simple = (g.simple_modes & 1 << switchPosition); - Serial.printf("do simple: %d \n", (int)do_simple); + #if CH7_OPTION != SIMPLE_MODE_CONTROL + // setup Simple mode + // do we enable simple mode? + do_simple = (g.simple_modes & 1 << switchPosition); + //Serial.printf("do simple: %d \n", (int)do_simple); + #endif + }else{ switch_debouncer = true; @@ -56,6 +59,7 @@ static void read_trim_switch() #elif CH7_OPTION == SIMPLE_MODE_CONTROL do_simple = (g.rc_7.control_in > 800); + //Serial.println(g.rc_7.control_in, DEC); #elif CH7_OPTION == DO_SET_HOVER // switch is engaged diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 1bbd29cbcf..498fc91bcf 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -14,8 +14,7 @@ #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 -#define ROLL_PITCH_SIMPLE 2 -#define ROLL_PITCH_AUTO 3 +#define ROLL_PITCH_AUTO 2 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a40ef79f27..a11b96e894 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -30,16 +30,6 @@ static void navigate() // target_bearing is where we should be heading // -------------------------------------------- target_bearing = get_bearing(¤t_loc, &next_WP); - - // nav_bearing will include xtrac correction - // ----------------------------------------- - //xtrack_enabled = false; - - //if(xtrack_enabled){ - // crosstrack_correction = get_crosstrack_correction(); - //}else { - // crosstrack_correction = 0; - //} } static bool check_missed_wp() @@ -71,6 +61,9 @@ static void calc_location_error(struct Location *next_loc) lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH } + +// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0); + #define NAV_ERR_MAX 400 static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed) { @@ -105,15 +98,20 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed 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(temp); + if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){ + x_actual_speed = optflow.vlon * 10; + y_actual_speed = optflow.vlat * 10; + }else{ + x_actual_speed = (float)g_gps->ground_speed * sin(temp); + 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, -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(temp); x_rate_error = x_target_speed - x_actual_speed; x_rate_error = constrain(x_rate_error, -600, 600); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 4025730e8c..86b30c9de7 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -101,7 +101,7 @@ void output_min() static void read_radio() { if (APM_RC.GetState() == 1){ - + new_radio_frame = true; g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 552fbee763..e9c25cf4c2 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -304,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) byte _oldSwitchPosition = 0; byte mode = 0; - Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off")); + Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n")); print_hit_enter(); while(1){ @@ -341,7 +341,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) } // look for stick input - if (abs(g.rc_4.control_in) > 3000){ + if (g.rc_4.control_in > 3000){ g.simple_modes |= (1<<_switchPosition); // print new mode print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); @@ -349,7 +349,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) } // look for stick input - if (abs(g.rc_4.control_in) < 3000){ + if (g.rc_4.control_in < -3000){ g.simple_modes &= ~(1<<_switchPosition); // print new mode print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); @@ -361,6 +361,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) for (mode = 0; mode < 6; mode++) flight_modes[mode].save(); + g.simple_modes.save(); print_done(); report_flight_modes(); return (0); @@ -760,10 +761,6 @@ setup_optflow(uint8_t argc, const Menu::arg *argv) } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.optflow_enabled = false; - //} else if(argv[1].i > 10){ - // g.optflow_fov.set_and_save(argv[1].i); - // optflow.set_field_of_view(g.optflow_fov.get()); - }else{ Serial.printf_P(PSTR("\nOptions:[on, off]\n")); report_optflow(); @@ -978,13 +975,13 @@ print_radio_values() static void print_switch(byte p, byte m, bool b) { - Serial.printf_P(PSTR("Pos %d: "),p); + Serial.printf_P(PSTR("Pos %d:\t"),p); Serial.print(flight_mode_strings[m]); - Serial.printf_P(PSTR(", Simple: ")); + Serial.printf_P(PSTR(",\t\tSimple: ")); if(b) - Serial.printf_P(PSTR("T\n")); + Serial.printf_P(PSTR("ON\n")); else - Serial.printf_P(PSTR("F\n")); + Serial.printf_P(PSTR("OFF\n")); } static void diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 55f8cec2be..34d9acd1c4 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -204,12 +204,12 @@ static void init_ardupilot() if(g.compass_enabled) init_compass(); -#ifdef OPTFLOW_ENABLED + #ifdef OPTFLOW_ENABLED // init the optical flow sensor if(g.optflow_enabled) { init_optflow(); } -#endif + #endif // Logging: // -------- @@ -321,8 +321,7 @@ static void startup_ground(void) #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 -#define ROLL_PITCH_SIMPLE 2 -#define ROLL_PITCH_AUTO 3 +#define ROLL_PITCH_AUTO 2 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 @@ -489,7 +488,10 @@ init_compass() static void init_optflow() { - optflow.init(); + if( optflow.init() == false ) { + g.optflow_enabled = false; + //SendDebug("\nFailed to Init OptFlow "); + } optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 895b41d55b..ab6a17085f 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -871,6 +871,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) static int8_t test_optflow(uint8_t argc, const Menu::arg *argv) { + ///* if(g.optflow_enabled) { Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); print_hit_enter(); diff --git a/libraries/APM_BMP085/APM_BMP085.h b/libraries/APM_BMP085/APM_BMP085.h index 1263f3aa5d..e8a5085d0f 100644 --- a/libraries/APM_BMP085/APM_BMP085.h +++ b/libraries/APM_BMP085/APM_BMP085.h @@ -2,7 +2,7 @@ #define APM_BMP085_h #define TEMP_FILTER_SIZE 16 -#define PRESS_FILTER_SIZE 8 +#define PRESS_FILTER_SIZE 10 class APM_BMP085_Class { diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.cpp b/libraries/AP_RC_Channel/AP_RC_Channel.cpp index b23879e0fd..9c8c8a6ca4 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.cpp +++ b/libraries/AP_RC_Channel/AP_RC_Channel.cpp @@ -79,15 +79,15 @@ AP_RC_Channel::set_pwm(int pwm) //Serial.print("range "); control_in = pwm_to_range(); control_in = (control_in < dead_zone) ? 0 : control_in; - if (fabs(scale_output) > 0){ - control_in *= scale_output; - } }else{ control_in = pwm_to_angle(); control_in = (abs(control_in) < dead_zone) ? 0 : control_in; - if (fabs(scale_output) > 0){ - control_in *= scale_output; + + if(expo) { + long temp = control_in; + temp = (temp * temp) / (long)_high; + control_in = (int)((control_in >= 0) ? temp : -temp); } } } diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.h b/libraries/AP_RC_Channel/AP_RC_Channel.h index d39806d904..db5dc68cd3 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.h +++ b/libraries/AP_RC_Channel/AP_RC_Channel.h @@ -23,17 +23,13 @@ class AP_RC_Channel{ _high(1), _filter(true), _reverse(1), - dead_zone(0), - scale_output(1.0) - {} + dead_zone(0){} AP_RC_Channel() : _high(1), _filter(true), _reverse(1), - dead_zone(0), - scale_output(1.0) - {} + dead_zone(0){} // setup min and max radio values in CLI void update_min_max(); @@ -94,8 +90,6 @@ class AP_RC_Channel{ int16_t pwm_to_range(); int16_t range_to_pwm(); - float scale_output; - private: bool _filter; int8_t _reverse; From 0d31d775b8b37095e87bd7571f6981ea8da6cbcb Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 15 Sep 2011 23:41:15 -0700 Subject: [PATCH 058/100] cleanup added more ifdef for optflow --- ArduCopter/ArduCopter.pde | 4 +++- ArduCopter/config.h | 2 +- ArduCopter/navigation.pde | 24 +++++++++++------------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 69e92c4b7b..62b3d06bec 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -204,7 +204,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #endif #endif -#elif HIL_MODE == HIL_MODE_ATTITUDE +#if HIL_MODE == HIL_MODE_ATTITUDE #ifdef OPTFLOW_ENABLED AP_OpticalFlow_ADNS3080 optflow(&dcm); #endif @@ -617,6 +617,7 @@ static void medium_loop() medium_loopCounter++; + #ifdef OPTFLOW_ENABLED if(g.optflow_enabled){ optflow.read(); optflow.update_position(cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow @@ -626,6 +627,7 @@ static void medium_loop() Log_Write_Optflow(); } } + #endif if(GPS_enabled){ update_GPS(); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3f6937a374..79e19c1b05 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -210,7 +210,7 @@ #define OPTFLOW_ENABLED #endif -#define OPTFLOW_ENABLED +//#define OPTFLOW_ENABLED #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) # define OPTFLOW DISABLED diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a11b96e894..855761192f 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -97,14 +97,19 @@ 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 - if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){ - x_actual_speed = optflow.vlon * 10; - y_actual_speed = optflow.vlat * 10; - }else{ + #ifdef OPTFLOW_ENABLED + // calc the cos of the error to tell how fast we are moving towards the target in cm + if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){ + x_actual_speed = optflow.vlon * 10; + y_actual_speed = optflow.vlat * 10; + }else{ + x_actual_speed = (float)g_gps->ground_speed * sin(temp); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); + } + #else x_actual_speed = (float)g_gps->ground_speed * sin(temp); y_actual_speed = (float)g_gps->ground_speed * cos(temp); - } + #endif y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum @@ -128,13 +133,6 @@ static void calc_nav_pitch_roll() nav_pitch = -nav_pitch; } -// ------------------------------ -/*static void calc_bearing_error() -{ - bearing_error = nav_bearing - dcm.yaw_sensor; - bearing_error = wrap_180(bearing_error); -}*/ - static long get_altitude_error() { return next_WP.alt - current_loc.alt; From 185bfc232a0fb0518868cc5469ae45d105875cf2 Mon Sep 17 00:00:00 2001 From: analoguedevices Date: Fri, 16 Sep 2011 08:13:33 +0000 Subject: [PATCH 059/100] comment change to test Git pull --- Tools/ArduTracker/APM_Config.h.reference | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ArduTracker/APM_Config.h.reference b/Tools/ArduTracker/APM_Config.h.reference index 0964e16769..3eb4c9c3c4 100644 --- a/Tools/ArduTracker/APM_Config.h.reference +++ b/Tools/ArduTracker/APM_Config.h.reference @@ -2,7 +2,7 @@ // Example and reference ArduPilot Mega configuration file // ======================================================= // -// This file contains documentation and examples for configuration options +// This file contains documentation and examples of configuration options // supported by the ArduPilot Mega software. // // Most of these options are just that - optional. You should create From 024512e34a1ba0a4ff8f50ba13ddb1aeb8f74ecb Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 16:54:45 -0700 Subject: [PATCH 060/100] Fixed CIRCLE mode trig error made loiter_radius * 100 in Params updated gains --- ArduCopter/ArduCopter.pde | 34 +++++++++++++++++++++++++--------- ArduCopter/Log.pde | 16 +++++++--------- ArduCopter/Parameters.h | 2 +- ArduCopter/config.h | 12 ++++++------ ArduCopter/motors_quad.pde | 1 + 5 files changed, 40 insertions(+), 25 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 62b3d06bec..0691b88635 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -345,6 +345,7 @@ static float cos_pitch_x = 1; static float cos_yaw_x = 1; static float sin_pitch_y, sin_yaw_y, sin_roll_y; static long initial_simple_bearing; // used for Simple mode +static float simple_sin_y, simple_cos_x; static float boost; // used to give a little extra to maintain altitude // Acro @@ -1043,17 +1044,29 @@ void update_roll_pitch_mode(void) if(do_simple && new_radio_frame){ new_radio_frame = false; - //Serial.printf("1: %d, 2: %d ",g.rc_1.control_in, g.rc_2.control_in); + simple_timer++; + + int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + + if (simple_timer == 1){ + // roll + simple_cos_x = sin(radians(90 - delta)); + + }else if (simple_timer > 2){ + // pitch + simple_sin_y = cos(radians(90 - delta)); + simple_timer = 0; + } // Rotate input by the initial bearing - control_roll = g.rc_1.control_in * sin_yaw_y + g.rc_2.control_in * cos_yaw_x; - control_pitch = g.rc_2.control_in * sin_yaw_y - g.rc_1.control_in * cos_yaw_x; + control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; + control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); g.rc_1.control_in = control_roll; g.rc_2.control_in = control_pitch; - //Serial.printf("\t1: %d, 2: %d\n",g.rc_1.control_in, g.rc_2.control_in); } + switch(roll_pitch_mode){ case ROLL_PITCH_ACRO: // Roll control @@ -1085,13 +1098,16 @@ void update_throttle_mode(void) { switch(throttle_mode){ case THROTTLE_MANUAL: - g.rc_3.servo_out = g.rc_3.control_in + boost; + if (g.rc_3.control_in > 0){ + g.rc_3.servo_out = g.rc_3.control_in + boost; + }else{ + g.rc_3.servo_out = 0; + } break; case THROTTLE_HOLD: // allow interactive changing of atitude adjust_altitude(); - // fall through case THROTTLE_AUTO: @@ -1170,10 +1186,10 @@ static void update_navigation() update_auto_yaw(); { //circle_angle += dTnav; //1000 * (dTnav/1000); - circle_angle = wrap_360(target_bearing + 2000 + 18000); + circle_angle = wrap_360(target_bearing + 3000 + 18000); - target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(radians(90 - circle_angle))); - target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(radians(90 - circle_angle))); + target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100))); + target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100))); } // calc the lat and long error to the target diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d5a71cc5a9..98dbf41998 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -558,7 +558,7 @@ static void Log_Write_Nav_Tuning() static void Log_Read_Nav_Tuning() { - Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d\n"), + Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d\n"), DataFlash.ReadInt(), // distance DataFlash.ReadByte(), // wp_verify_byte DataFlash.ReadInt(), // target_bearing @@ -591,8 +591,6 @@ static void Log_Read_Nav_Tuning() DataFlash.ReadInt(), DataFlash.ReadInt()); //nav bearing */ - - } @@ -630,8 +628,8 @@ static void Log_Read_Control_Tuning() "%d, %d\n"), // Control - DataFlash.ReadInt(), - DataFlash.ReadInt(), + //DataFlash.ReadInt(), + //DataFlash.ReadInt(), // yaw DataFlash.ReadInt(), @@ -760,7 +758,7 @@ static void Log_Write_Attitude() // Read an attitude packet static void Log_Read_Attitude() { - Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, $d\n"), + Serial.printf_P(PSTR("ATT, %d, %d, %u, %d, %d, %d\n"), DataFlash.ReadInt(), DataFlash.ReadInt(), (uint16_t)DataFlash.ReadInt(), @@ -821,15 +819,15 @@ static void Log_Read(int start_page, int end_page) case 0: if(data == HEAD_BYTE1) // Head byte 1 log_step++; - else - Serial.println("."); break; case 1: if(data == HEAD_BYTE2) // Head byte 2 log_step++; - else + else{ log_step = 0; + Serial.println("."); + } break; case 2: diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c6e6d7cc67..d026d30063 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -306,7 +306,7 @@ public: waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_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")), + loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 79e19c1b05..75cbfd2c03 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -392,7 +392,7 @@ // Attitude Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 3.6 +# define STABILIZE_ROLL_P 4.0 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.02 @@ -402,7 +402,7 @@ #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 3.6 +# define STABILIZE_PITCH_P 4.0 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.02 @@ -474,7 +474,7 @@ # define LOITER_P .4 // #endif #ifndef LOITER_I -# define LOITER_I 0.01 // +# define LOITER_I 0.10 // #endif #ifndef LOITER_IMAX # define LOITER_IMAX 12 // degrees° @@ -484,7 +484,7 @@ # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch #endif #ifndef NAV_I -# define NAV_I 0.12 // this +# define NAV_I 0.15 // this #endif #ifndef NAV_IMAX # define NAV_IMAX 20 // degrees @@ -654,8 +654,8 @@ # define WP_RADIUS_DEFAULT 3 #endif -#ifndef LOITER_RADIUS_DEFAULT -# define LOITER_RADIUS_DEFAULT 10 +#ifndef LOITER_RADIUS +# define LOITER_RADIUS 10 #endif #ifndef ALT_HOLD_HOME diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index d5af159ad4..5344a08f63 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -4,6 +4,7 @@ static void output_motors_armed() { + int roll_out, pitch_out; int out_min = g.rc_3.radio_min; int out_max = g.rc_3.radio_max; From 4f6ac5c4030972dda8641c6004d68f554583d0e9 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 18:24:42 -0700 Subject: [PATCH 061/100] Lowered Loiter_I - higher value didn't test well. --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 75cbfd2c03..3adcd5a07a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -474,7 +474,7 @@ # define LOITER_P .4 // #endif #ifndef LOITER_I -# define LOITER_I 0.10 // +# define LOITER_I 0.04 // #endif #ifndef LOITER_IMAX # define LOITER_IMAX 12 // degrees° From 3d307c9fde03cce042d514f831caf2313964cf03 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 18:56:51 -0700 Subject: [PATCH 062/100] Added low voltage code flashing code. --- ArduCopter/Parameters.h | 5 ++++- ArduCopter/defines.h | 2 +- ArduCopter/leds.pde | 41 +++++++++++++++++++++++++---------------- ArduCopter/sensors.pde | 5 ++++- 4 files changed, 34 insertions(+), 19 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index d026d30063..d9a5bca530 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/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 = 107; + static const uint16_t k_format_version = 108; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -78,6 +78,7 @@ public: k_param_top_bottom_ratio, k_param_optflow_enabled, k_param_input_voltage, + k_param_low_voltage, // // 160: Navigation parameters @@ -237,6 +238,7 @@ public: AP_Float top_bottom_ratio; AP_Int8 optflow_enabled; AP_Float input_voltage; + AP_Float low_voltage; #if FRAME_CONFIG == HELI_FRAME // Heli @@ -300,6 +302,7 @@ public: 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")), + low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_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 498fc91bcf..f10c432b2a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -52,7 +52,7 @@ #define FR_LED AN12 // Mega PE4 pin, OUT7 #define RE_LED AN14 // Mega PE5 pin, OUT6 #define RI_LED AN10 // Mega PH4 pin, OUT5 -#define LE_LED AN8 // Mega PH5 pin, OUT4 +#define LE_LED AN8 // Mega PH5 pin, OUT4 // Internal defines, don't edit and expect things to work // ------------------------------------------------------- diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 546dfeeb88..dd7a584bcc 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -96,26 +96,35 @@ static void clear_leds() #if MOTOR_LEDS == 1 static void update_motor_leds(void) { - // blink rear - static bool blink = false; + if (motor_armed == true){ + if (low_batt == true){ + // blink rear + static bool blink = false; - if (blink){ - digitalWrite(RE_LED, HIGH); - digitalWrite(FR_LED, HIGH); - digitalWrite(RI_LED, LOW); - digitalWrite(LE_LED, LOW); - }else{ + if (blink){ + digitalWrite(RE_LED, HIGH); + digitalWrite(FR_LED, HIGH); + digitalWrite(RI_LED, LOW); + digitalWrite(LE_LED, LOW); + }else{ + digitalWrite(RE_LED, LOW); + digitalWrite(FR_LED, LOW); + digitalWrite(RI_LED, HIGH); + digitalWrite(LE_LED, HIGH); + } + blink = !blink; + }else{ + digitalWrite(RE_LED, HIGH); + digitalWrite(FR_LED, HIGH); + digitalWrite(RI_LED, HIGH); + digitalWrite(LE_LED, HIGH); + } + }else { digitalWrite(RE_LED, LOW); digitalWrite(FR_LED, LOW); - digitalWrite(RI_LED, HIGH); - digitalWrite(LE_LED, HIGH); + digitalWrite(RI_LED, LOW); + digitalWrite(LE_LED, LOW); } - - blink = !blink; - - // the variable low_batt is here to let people know the voltage is low or the pack capacity is finished - // I don't know what folks want here. - // low_batt } #endif diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 7e928276f0..844044aa97 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -113,17 +113,20 @@ static void read_battery(void) if(g.battery_monitoring == 1) battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream + if(g.battery_monitoring == 2) battery_voltage = battery_voltage4; + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) battery_voltage = battery_voltage1; + if(g.battery_monitoring == 4) { current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; } #if BATTERY_EVENT == 1 - if(battery_voltage < LOW_VOLTAGE) + if(battery_voltage < g.low_voltage) low_battery_event(); if(g.battery_monitoring == 4 && current_total > g.pack_capacity) From cf58c3c88dfa56fb57dc97e111b1a9d8823d7b4c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 18:57:35 -0700 Subject: [PATCH 063/100] removed unused pde file --- ArduCopter/flight_control.pde | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 ArduCopter/flight_control.pde diff --git a/ArduCopter/flight_control.pde b/ArduCopter/flight_control.pde deleted file mode 100644 index e69de29bb2..0000000000 From 1f52ad27d969b12cd4cfca3622738108659cd46b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 19:06:53 -0700 Subject: [PATCH 064/100] roughed in an expo option idea made min and max values 1100, 1900 removed hard coded init for camera values --- ArduCopter/Camera.pde | 12 +----------- libraries/RC_Channel/RC_Channel.cpp | 6 ++++++ libraries/RC_Channel/RC_Channel.h | 4 ++-- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index ed49230bb5..e204d747fb 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -4,19 +4,9 @@ static void init_camera() { + // ch 6 high(right) is down. g.rc_camera_pitch.set_angle(4500); - g.rc_camera_pitch.radio_min = 1200; - g.rc_camera_pitch.radio_trim = 1500; - g.rc_camera_pitch.radio_max = 1900; - //g.rc_camera_pitch.set_reverse(1); - - // ch 6 high right is down. - - 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; g.rc_camera_roll.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_camera_pitch.set_type(RC_CHANNEL_ANGLE_RAW); diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index aa189dff15..0d41ee6ad8 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -98,6 +98,12 @@ RC_Channel::set_pwm(int pwm) //if (fabs(scale_output) > 0){ // control_in *= scale_output; //} + /* + if(expo) { + long temp = control_in; + temp = (temp * temp) / (long)_high; + control_in = (int)((control_in >= 0) ? temp : -temp); + }*/ } } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 132e9146f3..922b15540e 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -22,9 +22,9 @@ class RC_Channel{ /// RC_Channel(AP_Var::Key key, const prog_char_t *name) : _group(key, name), - radio_min (&_group, 0, 1500, name ? PSTR("MIN") : 0), // suppress name if group has no name + radio_min (&_group, 0, 1100, name ? PSTR("MIN") : 0), // suppress name if group has no name radio_trim(&_group, 1, 1500, name ? PSTR("TRIM") : 0), - radio_max (&_group, 2, 1500, name ? PSTR("MAX") : 0), + radio_max (&_group, 2, 1900, name ? PSTR("MAX") : 0), _high(1), _filter(true), _reverse (&_group, 3, 1, name ? PSTR("REV") : 0), From 41dada23ea277bfdf7744d40ec619c8399bd898e Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 19:17:50 -0700 Subject: [PATCH 065/100] removed DCM reference from OPT FLOW --- ArduCopter/ArduCopter.pde | 12 +++--------- ArduCopter/config.h | 2 +- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0691b88635..0f0a93c438 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -168,7 +168,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1; AP_Compass_HIL compass; // never used AP_IMU_Shim imu; // never used #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow(&dcm); + AP_OpticalFlow_ADNS3080 optflow; #endif static int32_t gps_base_alt; #else @@ -200,16 +200,10 @@ static AP_Int8 *flight_modes = &g.flight_mode1; AP_DCM dcm(&imu, g_gps); #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow(&dcm); + AP_OpticalFlow_ADNS3080 optflow(); #endif #endif -#if HIL_MODE == HIL_MODE_ATTITUDE - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow(&dcm); - #endif -#endif - //////////////////////////////////////////////////////////////////////////////// // GCS selection //////////////////////////////////////////////////////////////////////////////// @@ -621,7 +615,7 @@ static void medium_loop() #ifdef OPTFLOW_ENABLED if(g.optflow_enabled){ optflow.read(); - optflow.update_position(cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow // write to log if (g.log_bitmask & MASK_LOG_OPTFLOW){ diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3adcd5a07a..d2e40adec0 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -207,7 +207,7 @@ ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included - #define OPTFLOW_ENABLED + //#define OPTFLOW_ENABLED #endif //#define OPTFLOW_ENABLED From 5d61e9289cfbf97d0823eae85adc9fa8f7f06cfb Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 19:24:57 -0700 Subject: [PATCH 066/100] added sin and cos yaw from DCM, renamed "get" function because it doesn't return a value. --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 86 ++++++------ libraries/AP_OpticalFlow/AP_OpticalFlow.h | 22 ++-- .../AP_OpticalFlow_ADNS3080.cpp | 122 +++++++++--------- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 33 ++--- 4 files changed, 140 insertions(+), 123 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 67d2821ac0..2613acae3e 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -12,15 +12,11 @@ #define FORTYFIVE_DEGREES 0.78539816 -AP_OpticalFlow::AP_OpticalFlow() : raw_dx(0),raw_dy(0),x(0),y(0),surface_quality(0),dx(0),dy(0),last_update(0),field_of_view(1),scaler(0),num_pixels(0) -{ -} - // init - initCommAPI parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) -bool +bool AP_OpticalFlow::init(bool initCommAPI) { - _orientation_matrix = Matrix3f(1,0,0,0,1,0,0,0,1); + _orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); update_conversion_factors(); return true; // just return true by default } @@ -38,28 +34,28 @@ int AP_OpticalFlow::read() } // reads a value from the sensor (will be sensor specific) -byte +byte AP_OpticalFlow::read_register(byte address) { } // writes a value to one of the sensor's register (will be sensor specific) -void +void AP_OpticalFlow::write_register(byte address, byte value) { } // rotate raw values to arrive at final x,y,dx and dy values -void +void AP_OpticalFlow::apply_orientation_matrix() { Vector3f rot_vector; - + // next rotate dx and dy - rot_vector = _orientation_matrix * Vector3f(raw_dx,raw_dy,0); + rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0); dx = rot_vector.x; dy = rot_vector.y; - + // add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch) x += dx; y += dy; @@ -69,30 +65,26 @@ AP_OpticalFlow::apply_orientation_matrix() void AP_OpticalFlow::update_conversion_factors() { - conv_factor = (1.0/(float)(num_pixels*scaler))*2.0*tan(field_of_view/2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) - radians_to_pixels = (num_pixels*scaler) / field_of_view; + conv_factor = (1.0 / (float)(num_pixels * scaler)) * 2.0 * tan(field_of_view / 2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) + // 0.00615 + radians_to_pixels = (num_pixels * scaler) / field_of_view; + // 162.99 } // updates internal lon and lat with estimation based on optical flow void -AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude) +AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude) { - float diff_roll = roll - _last_roll; - float diff_pitch = pitch - _last_pitch; - float avg_altitude = (altitude + _last_altitude)/2; - //float exp_change_x, exp_change_y; - //float change_x, change_y; - //float x_cm, y_cm; - float cos_yaw = cos(yaw); - float sin_yaw = sin(yaw); - int i; - + float diff_roll = roll - _last_roll; + float diff_pitch = pitch - _last_pitch; + // only update position if surface quality is good and angle is not over 45 degrees if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) { + altitude = max(altitude, 0); // calculate expected x,y diff due to roll and pitch change exp_change_x = diff_roll * radians_to_pixels; exp_change_y = -diff_pitch * radians_to_pixels; - + // real estimated raw change from mouse change_x = dx - exp_change_x; change_y = dy - exp_change_y; @@ -100,14 +92,36 @@ AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude) // convert raw change to horizontal movement in cm x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less - - // use yaw to convert x and y into lon and lat - lat += y_cm * cos_yaw - x_cm * sin_yaw; - lng += x_cm * cos_yaw + y_cm * sin_yaw; + + + vlon = x_cm * sin_yaw_y - y_cm * cos_yaw_x; + vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; } - - // capture roll and pitch for next iteration - _last_roll = roll; - _last_pitch = pitch; - _last_altitude = altitude; -} \ No newline at end of file +} + + +/* +{ + // only update position if surface quality is good and angle is not over 45 degrees + if( surface_quality >= 10 && fabs(_dcm->roll) <= FORTYFIVE_DEGREES && fabs(_dcm->pitch) <= FORTYFIVE_DEGREES ) { + altitude = max(altitude, 0); + Vector3f omega = _dcm->get_gyro(); + + // calculate expected x,y diff due to roll and pitch change + float exp_change_x = omega.x * radians_to_pixels; + float exp_change_y = -omega.y * radians_to_pixels; + + // real estimated raw change from mouse + float change_x = dx - exp_change_x; + float change_y = dy - exp_change_y; + + // convert raw change to horizontal movement in cm + float x_cm = -change_x * altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? + float y_cm = -change_y * altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less + + vlon = (float)x_cm * sin_yaw_y - (float)y_cm * cos_yaw_x; + vlat = (float)y_cm * sin_yaw_y - (float)x_cm * cos_yaw_x; + } +} + +*/ \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index dec1d0bc28..79f1bb215f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -15,11 +15,11 @@ read : reads latest value from OpticalFlow and stores values in x,y, surface_quality parameter read_register() : reads a value from the sensor (will be sensor specific) write_register() : writes a value to one of the sensor's register (will be sensor specific) - */ -#include "WProgram.h" #include +#include +#include // return value definition #define OPTICALFLOW_FAIL 0 @@ -37,12 +37,12 @@ class AP_OpticalFlow { - public: + public: int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated) int surface_quality; // image quality (below 15 you really can't trust the x,y values returned) int x,y; // total x,y position int dx,dy; // rotated change in x and y position - float lng, lat; // position as offsets from original position + float vlon, vlat; // position as offsets from original position unsigned long last_update; // millis() time of last update float field_of_view; // field of view in Radians float scaler; // number returned from sensor when moved one pixel @@ -50,17 +50,19 @@ class AP_OpticalFlow // temp variables - delete me! float exp_change_x, exp_change_y; float change_x, change_y; - float x_cm, y_cm; - public: - AP_OpticalFlow(); // Constructor + float x_cm, y_cm; + + //AP_OpticalFlow(AP_DCM *dcm); virtual bool init(bool initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) virtual byte read_register(byte address); virtual void write_register(byte address, byte value); virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame. virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor virtual int read(); // read latest values from sensor and fill in x,y and totals - virtual void get_position(float roll, float pitch, float yaw, float altitude); // updates internal lon and lat with estimation based on optical flow - + virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow + + + //protected: Matrix3f _orientation_matrix; float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) @@ -70,6 +72,4 @@ class AP_OpticalFlow virtual void update_conversion_factors(); }; -#include "AP_OpticalFlow_ADNS3080.h" - #endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 15a2c35cb7..f42d1acb9c 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -33,18 +33,18 @@ union NumericIntType { int intValue; unsigned int uintValue; - byte byteValue[2]; + byte byteValue[2]; }; - -// Constructors //////////////////////////////////////////////////////////////// + // Constructors //////////////////////////////////////////////////////////////// AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080() { - num_pixels = ADNS3080_PIXELS_X; - field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; + num_pixels = ADNS3080_PIXELS_X; + field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV; scaler = AP_OPTICALFLOW_ADNS3080_SCALER; } + // Public Methods ////////////////////////////////////////////////////////////// // init - initialise sensor // initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface) @@ -52,7 +52,7 @@ bool AP_OpticalFlow_ADNS3080::init(bool initCommAPI) { int retry = 0; - + pinMode(AP_SPI_DATAOUT,OUTPUT); pinMode(AP_SPI_DATAIN,INPUT); pinMode(AP_SPI_CLOCK,OUTPUT); @@ -63,12 +63,12 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // reset the device reset(); - + // start the SPI library: if( initCommAPI ) { SPI.begin(); } - + // check the sensor is functioning if( retry < 3 ) { if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) @@ -84,16 +84,16 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) // byte AP_OpticalFlow_ADNS3080::backup_spi_settings() -{ +{ // store current spi values orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA); orig_spi_settings_spsr = SPSR & SPI2X; - + // set the values that we need SPI.setBitOrder(MSBFIRST); SPI.setDataMode(SPI_MODE3); - SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed - + SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed + return orig_spi_settings_spcr; } @@ -102,7 +102,7 @@ byte AP_OpticalFlow_ADNS3080::restore_spi_settings() { byte temp; - + // restore SPSR temp = SPSR; temp &= ~SPI2X; @@ -114,35 +114,35 @@ AP_OpticalFlow_ADNS3080::restore_spi_settings() temp &= ~(DORD | CPOL | CPHA); // zero out the important bits temp |= orig_spi_settings_spcr; // restore important bits SPCR = temp; - + return temp; } // Read a register from the sensor byte -AP_OpticalFlow_ADNS3080::read_register(byte address) +AP_OpticalFlow_ADNS3080::read_register(byte address) { byte result = 0, junk = 0; backup_spi_settings(); - + // take the chip select low to select the device digitalWrite(ADNS3080_CHIP_SELECT, LOW); - + // send the device the register you want to read: junk = SPI.transfer(address); - + // small delay delayMicroseconds(50); - + // send a value of 0 to read the first byte returned: result = SPI.transfer(0x00); - + // take the chip select high to de-select: digitalWrite(ADNS3080_CHIP_SELECT, HIGH); - + restore_spi_settings(); - + return result; } @@ -152,24 +152,24 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value) { byte junk = 0; - backup_spi_settings(); - + backup_spi_settings(); + // take the chip select low to select the device digitalWrite(ADNS3080_CHIP_SELECT, LOW); - + // send register address junk = SPI.transfer(address | 0x80 ); - + // small delay - delayMicroseconds(50); - + delayMicroseconds(50); + // send data junk = SPI.transfer(value); - + // take the chip select high to de-select: digitalWrite(ADNS3080_CHIP_SELECT, HIGH); - - restore_spi_settings(); + + restore_spi_settings(); } // reset sensor by holding a pin high (or is it low?) for 10us. @@ -187,7 +187,7 @@ AP_OpticalFlow_ADNS3080::read() { surface_quality = (unsigned int)read_register(ADNS3080_SQUAL); delayMicroseconds(50); // small delay - + // check for movement, update x,y values if( (read_register(ADNS3080_MOTION) & 0x80) != 0 ) { raw_dx = ((char)read_register(ADNS3080_DELTA_X)); @@ -198,11 +198,11 @@ AP_OpticalFlow_ADNS3080::read() raw_dx = 0; raw_dy = 0; } - + last_update = millis(); - + apply_orientation_matrix(); - + return OPTICALFLOW_SUCCESS; } @@ -238,13 +238,13 @@ void AP_OpticalFlow_ADNS3080::set_resolution(int resolution) { byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); - + if( resolution == ADNS3080_RESOLUTION_400 ) { regVal &= ~0x10; - }else if( resolution == ADNS3080_RESOLUTION_1200) { + }else if( resolution == ADNS3080_RESOLUTION_1200) { regVal |= 0x10; } - + delayMicroseconds(50); // small delay write_register(ADNS3080_CONFIGURATION_BITS, regVal); } @@ -273,7 +273,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate) delayMicroseconds(50); // small delay write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A); delayMicroseconds(50); // small delay - + // decide what value to update in extended config regVal = (regVal & ~0x01); }else{ @@ -287,7 +287,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate) unsigned int AP_OpticalFlow_ADNS3080::get_frame_period() { - NumericIntType aNum; + NumericIntType aNum; aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); delayMicroseconds(50); // small delay aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER); @@ -300,11 +300,11 @@ AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period) { NumericIntType aNum; aNum.uintValue = period; - + // set frame rate to manual set_frame_rate_auto(false); delayMicroseconds(50); // small delay - + // set specific frame period write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]); delayMicroseconds(50); // small delay @@ -325,7 +325,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate) { unsigned long clockSpeed = ADNS3080_CLOCK_SPEED; unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate); - + set_frame_period(period); } @@ -349,11 +349,11 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) delayMicroseconds(50); // small delay if( auto_shutter_speed ) { // return shutter speed max to default - write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); - delayMicroseconds(50); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c); + delayMicroseconds(50); // small delay write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20); - delayMicroseconds(50); // small delay - + delayMicroseconds(50); // small delay + // determine value to put into extended config regVal = regVal & ~0x02; }else{ @@ -361,41 +361,41 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) regVal = regVal & ~0x02 | 0x02; } write_register(ADNS3080_EXTENDED_CONFIG, regVal); - delayMicroseconds(50); // small delay + delayMicroseconds(50); // small delay } // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual unsigned int AP_OpticalFlow_ADNS3080::get_shutter_speed() { - NumericIntType aNum; + NumericIntType aNum; aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER); delayMicroseconds(50); // small delay aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER); return aNum.uintValue; } - - + + // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed) { NumericIntType aNum; aNum.uintValue = shutter_speed; - + // set shutter speed to manual set_shutter_speed_auto(false); delayMicroseconds(50); // small delay - + // set specific shutter speed - write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); - delayMicroseconds(50); // small delay + write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]); + delayMicroseconds(50); // small delay write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]); delayMicroseconds(50); // small delay - + // larger delay delay(50); - + // need to update frame period to cause shutter value to take effect aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER); delayMicroseconds(50); // small delay @@ -427,13 +427,13 @@ AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) bool isFirstPixel = true; byte regValue; byte pixelValue; - + // write to frame capture register to force capture of frame write_register(ADNS3080_FRAME_CAPTURE,0x83); - + // wait 3 frame periods + 10 nanoseconds for frame to be captured delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510 - + // display the pixel data for( i=0; iprintln(); } - + // hardware reset to restore sensor to normal operation reset(); } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index 41d233ebae..e5c296f186 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -1,8 +1,10 @@ #ifndef AP_OPTICALFLOW_ADNS3080_H #define AP_OPTICALFLOW_ADNS3080_H -#include -#include +//#include +//#include +//#include +//#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h" // orientations for ADNS3080 sensor @@ -29,10 +31,10 @@ #define ADNS3080_CHIP_SELECT 34 // PC3 #define ADNS3080_RESET 40 // PG1 was 35 / PC2 #else // normal arduino SPI pins...these need to be checked - #define AP_SPI_DATAIN 12 //MISO + #define AP_SPI_DATAIN 12 //MISO #define AP_SPI_DATAOUT 11 //MOSI #define AP_SPI_CLOCK 13 //SCK - #define ADNS3080_CHIP_SELECT 10 //SS + #define ADNS3080_CHIP_SELECT 10 //SS #define ADNS3080_RESET 9 //RESET #endif @@ -91,39 +93,40 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow // bytes to store SPI settings byte orig_spi_settings_spcr; byte orig_spi_settings_spsr; - + // save and restore SPI settings byte backup_spi_settings(); byte restore_spi_settings(); - + bool _motion; // true if there has been motion - + public: - AP_OpticalFlow_ADNS3080(); // Constructor + AP_OpticalFlow_ADNS3080(); + //AP_OpticalFlow_ADNS3080(); // Constructor bool init(bool initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) byte read_register(byte address); void write_register(byte address, byte value); void reset(); // reset sensor by holding a pin high (or is it low?) for 10us. int read(); // read latest values from sensor and fill in x,y and totals, return OPTICALFLOW_SUCCESS on successful read - + // ADNS3080 specific features bool motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } // return true if there has been motion since the last time this was called - + bool get_led_always_on(); // returns true if LED is always on, false if only on when required void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required - + int get_resolution(); // returns resolution (either 400 or 1200 counts per inch) void set_resolution(int resolution); // set parameter to 400 or 1200 counts per inch - + bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false) - unsigned int get_frame_period(); // get_frame_period - + unsigned int get_frame_period(); // get_frame_period - void set_frame_period(unsigned int period); unsigned int get_frame_rate(); void set_frame_rate(unsigned int rate); - + bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) @@ -131,7 +134,7 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow unsigned int set_shutter_speed(unsigned int shutter_speed); void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared - + int print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port }; From 43e77222ffa69156240aa86e128d4683351fe631 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 16 Sep 2011 19:26:17 -0700 Subject: [PATCH 067/100] fixed naming for opt flow --- ArduCopter/Log.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 98dbf41998..232f60e9dd 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -503,8 +503,8 @@ static void Log_Write_Optflow() DataFlash.WriteInt((int)optflow.dx); DataFlash.WriteInt((int)optflow.dy); DataFlash.WriteInt((int)optflow.surface_quality); - DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); - DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); + DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat); + DataFlash.WriteLong(optflow.lon);//optflow_offset.lng + optflow.lng); DataFlash.WriteByte(END_BYTE); } #endif From 89f664e3b0dd8568326790b93063a6f7100dd475 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 11 Sep 2011 09:39:35 +1000 Subject: [PATCH 068/100] barometer: print a message on barometer init in example code this helps track down initialisation problems --- .../examples/APM_BMP085_test/APM_BMP085_test.pde | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde b/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde index bab2661b19..4d2d23fd16 100644 --- a/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde +++ b/libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde @@ -12,9 +12,11 @@ unsigned long timer; void setup() { - APM_BMP085.Init(); // APM ADC initialization - Serial.begin(38400); + Serial.begin(115200); Serial.println("ArduPilot Mega BMP085 library test"); + Serial.println("Initialising barometer..."); delay(100); + APM_BMP085.Init(); // APM ADC initialization + Serial.println("initialisation complete."); delay(100); delay(1000); timer = millis(); } @@ -28,11 +30,11 @@ void loop() if((millis()- timer) > 50){ timer = millis(); APM_BMP085.Read(); - Serial.print("Pressure:"); + Serial.print("Pressure:"); Serial.print(APM_BMP085.Press); - Serial.print(" Temperature:"); + Serial.print(" Temperature:"); Serial.print(APM_BMP085.Temp / 10.0); - Serial.print(" Altitude:"); + Serial.print(" Altitude:"); tmp_float = (APM_BMP085.Press / 101325.0); tmp_float = pow(tmp_float, 0.190295); Altitude = 44330 * (1.0 - tmp_float); From 1b1f955b86e51e266d3bbe2697306a52c03d3f0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 13 Sep 2011 18:47:27 +1000 Subject: [PATCH 069/100] GPS: change example serial rate to 115200 this matches default for AP/AC --- libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index c4e14de3a1..e638f085f0 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -18,7 +18,7 @@ AP_GPS_Auto GPS(&Serial1, &gps); void setup() { - Serial.begin(38400); + Serial.begin(115200); Serial1.begin(38400); Serial.println("GPS AUTO library test"); From d34f8ceca6ffdc5ca5f182c6424b06aabaf1cf16 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 13 Sep 2011 18:54:00 +1000 Subject: [PATCH 070/100] MAVLink: only send HEARTBEAT and SYS_STATUS during initialisation I am concerned that some of the mavlink send routines may impact on sensor calibration, so its safer to just send the minimum information to keep the GCS happy --- ArduPlane/GCS_Mavlink.pde | 23 +++-------------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index b37ecb791f..bc0268ab66 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1085,15 +1085,6 @@ 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 @@ -1103,7 +1094,7 @@ static void send_rate(uint16_t low, uint16_t high) { static void mavlink_delay(unsigned long t) { unsigned long tstart; - static unsigned long last_1hz, last_3hz, last_10hz, last_50hz; + static unsigned long last_1hz, last_50hz; if (in_mavlink_delay) { // this should never happen, but let's not tempt fate by @@ -1120,18 +1111,11 @@ static void mavlink_delay(unsigned long t) if (tnow - last_1hz > 1000) { last_1hz = tnow; gcs.send_message(MSG_HEARTBEAT); + gcs.send_message(MSG_EXTENDED_STATUS1); #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_HEARTBEAT); + hil.send_message(MSG_EXTENDED_STATUS1); #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; @@ -1139,7 +1123,6 @@ static void mavlink_delay(unsigned long t) #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); From 8a31af801fa4d7fe54ef0d2b6e4398f007c99e46 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:01:45 +1000 Subject: [PATCH 071/100] ADC: re-work for ADC code for full resolution and accurate timing this changes the ADC code to return the full resolution of the sensors. It also adds a new Ch6() interface that returns 6 channels at once, so the IMU can read 3 accelerometers and 3 gyros at once, and get the exact time that the values were accumulated over --- libraries/AP_ADC/AP_ADC.h | 26 ++++-- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 123 +++++++++++++++++++--------- libraries/AP_ADC/AP_ADC_ADS7844.h | 8 +- libraries/AP_ADC/AP_ADC_HIL.cpp | 16 ++-- libraries/AP_ADC/AP_ADC_HIL.h | 20 +++-- 5 files changed, 133 insertions(+), 60 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index 196a6741c3..cafc460c1a 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -1,6 +1,8 @@ #ifndef AP_ADC_H #define AP_ADC_H +#include + /* AP_ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega Code by James Goppert. DIYDrones.com @@ -13,7 +15,7 @@ Methods: Init() : Initialization of ADC. (interrupts etc) Ch(ch_num) : Return the ADC channel value - + Ch6(channel_numbers, result) : Return 6 ADC channel values */ class AP_ADC @@ -21,11 +23,23 @@ class AP_ADC public: AP_ADC() {}; // Constructor virtual void Init() {}; - virtual int Ch(unsigned char ch_num) = 0; - virtual int Ch_raw(unsigned char ch_num) = 0; - private: -}; - + + /* read one channel value */ + virtual uint16_t Ch(uint8_t ch_num) = 0; + + /* read 6 channels values as a set, used by IMU for 3 gyros + and 3 accelerometeres. + + Pass in an array of 6 channel numbers and results are + returned in result[] + + The function returns the amount of time that the returned + value has been averaged over, in 2.5 millisecond units + */ + virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0; + private: +}; + #include "AP_ADC_ADS7844.h" #include "AP_ADC_HIL.h" diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 27bbc0a477..1625809755 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -44,6 +44,7 @@ extern "C" { // AVR LibC Includes #include + #include #include #include "WConstants.h" } @@ -53,8 +54,12 @@ extern "C" { // Commands for reading ADC channels on ADS7844 static const unsigned char adc_cmd[9] = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 }; -static volatile uint16_t _filter[8][ADC_FILTER_SIZE]; -static volatile uint8_t _filter_index; + +// the sum of the values since last read +static volatile uint32_t _sum[8]; + +// how many values we've accumulated since last read +static volatile uint16_t _count[8]; static unsigned char ADC_SPI_transfer(unsigned char data) { @@ -72,32 +77,33 @@ static unsigned char ADC_SPI_transfer(unsigned char data) ISR (TIMER2_OVF_vect) { uint8_t ch; - uint16_t adc_tmp; - //bit_set(PORTL,6); // To test performance + //bit_set(PORTL,6); // To test performance - bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) - ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel + bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) + ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel for (ch = 0; ch < 8; ch++){ - adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte + uint16_t adc_tmp; + adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command - // Fill our Moving average filter - _filter[ch][_filter_index] = adc_tmp >> 3; + _count[ch]++; + if (_count[ch] == 0) { + // overflow ... shouldn't happen too often + // unless we're just not using the + // channel. Notice that we overflow the count + // to 1 here, not zero, as otherwise the + // reader below could get a division by zero + _sum[ch] = 0; + _count[ch] = 1; + } + _sum[ch] += adc_tmp; } - // increment our filter - _filter_index++; - - // loop our filter - if(_filter_index == ADC_FILTER_SIZE) - _filter_index = 0; - - - bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) - //bit_clear(PORTL,6); // To test performance - TCNT2 = 104; // 400 Hz + bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) + //bit_clear(PORTL,6); // To test performance + TCNT2 = 104; // 400Hz interrupt } @@ -123,6 +129,16 @@ void AP_ADC_ADS7844::Init(void) // Set Baud rate UBRR2 = 2; // SPI clock running at 2.6MHz + // get an initial value for each channel. This ensures + // _count[] is never zero + for (uint8_t i=0; i<8; i++) { + uint16_t adc_tmp; + adc_tmp = ADC_SPI_transfer(0) << 8; + adc_tmp |= ADC_SPI_transfer(adc_cmd[i + 1]); + _count[i] = 1; + _sum[i] = adc_tmp; + } + // Enable Timer2 Overflow interrupt to capture ADC data TIMSK2 = 0; // Disable interrupts @@ -134,31 +150,60 @@ void AP_ADC_ADS7844::Init(void) } // Read one channel value -int AP_ADC_ADS7844::Ch(unsigned char ch_num) +uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) { - uint16_t result = 0; + uint16_t count; + uint32_t sum; - //while(adc_counter[ch_num] < 2) { } // Wait for at least 2 samples in accumlator + // ensure we have at least one value + while (_count[ch_num] == 0) /* noop */ ; - // stop interrupts + // grab the value with interrupts disabled, and clear the count cli(); - - // sum our filter - for(uint8_t i = 0; i < ADC_FILTER_SIZE; i++){ - result += _filter[ch_num][i]; + count = _count[ch_num]; + sum = _sum[ch_num]; + _count[ch_num] = 0; + _sum[ch_num] = 0; + sei(); + + return sum/count; +} + +// Read 6 channel values +// this assumes that the counts for all of the 6 channels are +// equal. This will only be true if we always consistently access a +// sensor by either Ch6() or Ch() and never mix them. If you mix them +// then you will get very strange results +uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) +{ + uint16_t count[6]; + uint32_t sum[6]; + uint8_t i; + + // ensure we have at least one value + for (i=0; i<6; i++) { + while (_count[channel_numbers[i]] == 0) /* noop */; } - // resume interrupts + // grab the values with interrupts disabled, and clear the counts + cli(); + for (i=0; i<6; i++) { + count[i] = _count[channel_numbers[i]]; + sum[i] = _sum[channel_numbers[i]]; + _count[channel_numbers[i]] = 0; + _sum[channel_numbers[i]] = 0; + } sei(); - // average our sampels - result /= ADC_FILTER_SIZE; - - return(result); -} - -// Read one channel value -int AP_ADC_ADS7844::Ch_raw(unsigned char ch_num) -{ - return _filter[ch_num][_filter_index]; // close enough + // calculate averages. We keep this out of the cli region + // to prevent us stalling the ISR while doing the + // division. That costs us 36 bytes of stack, but I think its + // worth it. + for (i=0; i<6; i++) { + result[i] = sum[i] / count[i]; + } + + // this assumes the count in all channels is equal, which will + // be true if the callers are using the interface consistently + return count[0]; } diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index ca748e4e1f..c4b99ecaed 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -19,8 +19,12 @@ class AP_ADC_ADS7844 : public AP_ADC public: AP_ADC_ADS7844(); // Constructor void Init(); - int Ch(unsigned char ch_num); - int Ch_raw(unsigned char ch_num); + + // Read 1 sensor value + uint16_t Ch(unsigned char ch_num); + + // Read 6 sensors at once + uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result); private: }; diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index 07cfdbd8f0..1684b161b7 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -35,6 +35,8 @@ AP_ADC_HIL::AP_ADC_HIL() // set diff press and temp to zero setGyroTemp(0); setPressure(0); + + last_hil_time = millis(); } void AP_ADC_HIL::Init(void) @@ -42,14 +44,18 @@ void AP_ADC_HIL::Init(void) } // Read one channel value -int AP_ADC_HIL::Ch(unsigned char ch_num) +uint16_t AP_ADC_HIL::Ch(unsigned char ch_num) { - return adcValue[ch_num]; + return adcValue[ch_num]; } -// Read one channel value -int AP_ADC_HIL::Ch_raw(unsigned char ch_num) + +// Read 6 channel values +uint16_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) { - return adcValue[ch_num]; + for (uint8_t i=0; i<6; i++) { + result[i] = Ch(channel_numbers[i]); + } + return ((millis() - last_hil_time)*2)/5; } // Set one channel value diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index 736e99a882..ed0c039289 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -32,10 +32,11 @@ class AP_ADC_HIL : public AP_ADC /// // Read the sensor, part of public AP_ADC interface - int Ch(unsigned char ch_num); + uint16_t Ch(unsigned char ch_num); + /// - // Read the sensor, part of public AP_ADC interface - int Ch_raw(unsigned char ch_num); + // Read 6 sensors at once + uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result); /// // Set the adc raw values given the current rotations rates, @@ -49,16 +50,19 @@ class AP_ADC_HIL : public AP_ADC // The raw adc array uint16_t adcValue[8]; + // the time in milliseconds when we last got a HIL update + uint32_t last_hil_time; + /// // sensor constants // constants declared in cpp file // @see AP_ADC_HIL.cpp - static const uint8_t sensors[6]; - static const float gyroBias[3]; - static const float gyroScale[3]; + static const uint8_t sensors[6]; + static const float gyroBias[3]; + static const float gyroScale[3]; static const float accelBias[3]; - static const float accelScale[3]; - static const int8_t sensorSign[6]; + static const float accelScale[3]; + static const int8_t sensorSign[6]; /// // gyro set function From bb35fdec1093ae320f463bef4cc83daf8c9733ba Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:03:18 +1000 Subject: [PATCH 072/100] IMU: re-work the IMU library to take advantage of the ADC Ch6() call This changes the IMU code to read 6 synchronised ADC channels at one time, giving us matching values, and exposing the exact averaging time to callers --- libraries/AP_IMU/AP_IMU_Oilpan.cpp | 47 ++++++++++++++++++++---------- libraries/AP_IMU/AP_IMU_Oilpan.h | 9 +++--- libraries/AP_IMU/IMU.h | 11 +++++++ 3 files changed, 47 insertions(+), 20 deletions(-) diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index 6a8f17c2e0..b332142520 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -77,6 +77,7 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t)) float prev[3] = {0,0,0}; float total_change; float max_offset; + uint16_t adc_values[6]; // cold start tc_temp = _adc->Ch(_gyro_temp_ch); @@ -88,8 +89,7 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t)) digitalWrite(C_LED_PIN, HIGH); callback(20); - for (int i = 0; i < 6; i++) - adc_in = _adc->Ch(_sensors[i]); + _adc->Ch6(_sensors, adc_values); digitalWrite(A_LED_PIN, HIGH); digitalWrite(C_LED_PIN, LOW); @@ -100,16 +100,23 @@ AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t)) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time do { + // get 6 sensor values + _adc->Ch6(_sensors, adc_values); + for (int j = 0; j <= 2; j++){ prev[j] = _sensor_cal[j]; - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; adc_in -= _sensor_compensation(j, tc_temp); _sensor_cal[j] = adc_in; } for(int i = 0; i < 50; i++){ + + // get 6 sensor values + _adc->Ch6(_sensors, adc_values); + for (int j = 0; j < 3; j++){ - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; // Subtract temp compensated typical gyro bias adc_in -= _sensor_compensation(j, tc_temp); // filter @@ -159,6 +166,7 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t)) float prev[6] = {0,0,0}; float total_change; float max_offset; + uint16_t adc_values[6]; // cold start callback(500); @@ -168,9 +176,11 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t)) for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time do { + _adc->Ch6(_sensors, adc_values); + for (int j = 3; j <= 5; j++){ prev[j] = _sensor_cal[j]; - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; adc_in -= _sensor_compensation(j, 0); // temperature ignored _sensor_cal[j] = adc_in; } @@ -179,8 +189,10 @@ AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t)) callback(20); + _adc->Ch6(_sensors, adc_values); + for (int j = 3; j < 6; j++){ - adc_in = _adc->Ch(_sensors[j]); + adc_in = adc_values[j]; adc_in -= _sensor_compensation(j, 0); // temperature ignored _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; } @@ -222,7 +234,7 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const // do gyro temperature compensation if (channel < 3) { - return 1658; + return 1658.0; /* return _gyro_temp_curve[channel][0] + _gyro_temp_curve[channel][1] * temperature + @@ -231,17 +243,17 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const } // do fixed-offset accelerometer compensation - return 2041; // Average raw value from a 20 board sample + return 2041.0; // Average raw value from a 20 board sample } float -AP_IMU_Oilpan::_sensor_in(uint8_t channel, int temperature) +AP_IMU_Oilpan::_sensor_in(uint8_t channel, uint16_t adc_value, int temperature) { float adc_in; // get the compensated sensor value // - adc_in = _adc->Ch(_sensors[channel]) - _sensor_compensation(channel, temperature); + adc_in = adc_value - _sensor_compensation(channel, temperature); // adjust for sensor sign and apply calibration offset // @@ -265,18 +277,21 @@ bool AP_IMU_Oilpan::update(void) { int tc_temp = _adc->Ch(_gyro_temp_ch); + uint16_t adc_values[6]; + + _ticks = _adc->Ch6(_sensors, adc_values); // convert corrected gyro readings to delta acceleration // - _gyro.x = ToRad(_gyro_gain_x) * _sensor_in(0, tc_temp); - _gyro.y = ToRad(_gyro_gain_y) * _sensor_in(1, tc_temp); - _gyro.z = ToRad(_gyro_gain_z) * _sensor_in(2, tc_temp); + _gyro.x = _gyro_gain_x * _sensor_in(0, adc_values[0], tc_temp); + _gyro.y = _gyro_gain_y * _sensor_in(1, adc_values[1], tc_temp); + _gyro.z = _gyro_gain_z * _sensor_in(2, adc_values[2], tc_temp); // convert corrected accelerometer readings to acceleration // - _accel.x = _accel_scale * _sensor_in(3, tc_temp); - _accel.y = _accel_scale * _sensor_in(4, tc_temp); - _accel.z = _accel_scale * _sensor_in(5, tc_temp); + _accel.x = _accel_scale * _sensor_in(3, adc_values[3], tc_temp); + _accel.y = _accel_scale * _sensor_in(4, adc_values[4], tc_temp); + _accel.z = _accel_scale * _sensor_in(5, adc_values[5], tc_temp); _accel_filtered.x = _accel_filtered.x * .98 + _accel.x * .02; _accel_filtered.y = _accel_filtered.y * .98 + _accel.y * .02; diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.h b/libraries/AP_IMU/AP_IMU_Oilpan.h index 90b683e300..5941f0a361 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.h +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -68,8 +68,8 @@ private: virtual void _init_accel(void (*callback)(unsigned long t)); ///< no-save implementation virtual void _init_gyro(void (*callback)(unsigned long t)); ///< no-save implementation + float _sensor_in(uint8_t channel, uint16_t adc_value, int temperature); float _sensor_compensation(uint8_t channel, int temp) const; - float _sensor_in(uint8_t channel, int temperature); // constants static const uint8_t _sensors[6]; ///< ADC channel mappings for the sensors @@ -83,14 +83,15 @@ private: // static const float _gravity = 423.8; ///< 1G in the raw data coming from the accelerometer // Value based on actual sample data from 20 boards + static const float _accel_scale = 9.80665 / 423.8; ///< would like to use _gravity here, but cannot // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : 0.4026, ?, 0.4192 // - static const float _gyro_gain_x = 0.4; // X axis Gyro gain - static const float _gyro_gain_y = 0.41; // Y axis Gyro gain - static const float _gyro_gain_z = 0.41; // Z axis Gyro gain + static const float _gyro_gain_x = ToRad(0.4); // X axis Gyro gain + static const float _gyro_gain_y = ToRad(0.41); // Y axis Gyro gain + static const float _gyro_gain_z = ToRad(0.41); // Z axis Gyro gain // Maximum possible value returned by an offset-corrected sensor channel // diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index 19754fc365..42a128ea58 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -70,12 +70,19 @@ public: /// Vector3f get_accel(void) { return _accel; } + /// Fetch the current accelerometer values /// /// @returns vector of current accelerations in m/s/s /// Vector3f get_accel_filtered(void) { return _accel_filtered; } + /// return the number of seconds that the last update represents + /// + /// @returns number of seconds + /// + float get_delta_time(void) { return _ticks * (2.5/1000.0); } + /// A count of bad sensor readings /// /// @todo This should be renamed, as there's no guarantee that sensors @@ -90,6 +97,10 @@ protected: /// Most recent gyro reading obtained by ::update Vector3f _gyro; + + /// number of 2.5ms ticks that the accel and gyro values + /// were calculated from + uint16_t _ticks; }; #endif From d96fbf8acff099098886297ba64b2c8137ca8165 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:04:08 +1000 Subject: [PATCH 073/100] DCM: re-work the DCM to use the new IMU/ADC features this gives us higher resolution DCM calculations, with much more accurate timing of the update delta time. --- libraries/AP_DCM/AP_DCM.cpp | 18 +++++++++++++----- libraries/AP_DCM/AP_DCM.h | 4 ++-- libraries/AP_DCM/AP_DCM_HIL.cpp | 2 +- libraries/AP_DCM/AP_DCM_HIL.h | 2 +- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index b6dcaaf4b1..4cbcdfeb02 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -10,7 +10,7 @@ version 2.1 of the License, or (at your option) any later version. Methods: - update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data + update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data get_gyro() : Returns gyro vector corrected for bias get_accel() : Returns accelerometer vector get_dcm_matrix() : Returns dcm matrix @@ -41,13 +41,17 @@ AP_DCM::set_compass(Compass *compass) /**************************************************/ void -AP_DCM::update_DCM_fast(float _G_Dt) +AP_DCM::update_DCM_fast(void) { + float delta_t; + _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 + delta_t = _imu->get_delta_time(); + + matrix_update(delta_t); // Integrate the DCM matrix switch(_toggle++){ case 0: @@ -82,13 +86,17 @@ AP_DCM::update_DCM_fast(float _G_Dt) /**************************************************/ void -AP_DCM::update_DCM(float _G_Dt) +AP_DCM::update_DCM(void) { + float delta_t; + _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 + delta_t = _imu->get_delta_time(); + + matrix_update(delta_t); // Integrate the DCM matrix normalize(); // Normalize the DCM matrix drift_correction(); // Perform drift correction euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 8e8af7cb36..503f2ab1fa 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -49,8 +49,8 @@ public: void set_compass(Compass *compass); // Methods - void update_DCM(float _G_Dt); - void update_DCM_fast(float _G_Dt); + void update_DCM(void); + void update_DCM_fast(void); float get_health(void); diff --git a/libraries/AP_DCM/AP_DCM_HIL.cpp b/libraries/AP_DCM/AP_DCM_HIL.cpp index 2597d9eb58..66adbbee7e 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.cpp +++ b/libraries/AP_DCM/AP_DCM_HIL.cpp @@ -8,7 +8,7 @@ version 2.1 of the License, or (at your option) any later version. Methods: - update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data + update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data get_gyro() : Returns gyro vector corrected for bias get_accel() : Returns accelerometer vector get_dcm_matrix() : Returns dcm matrix diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h index 447f4476f6..071370afa7 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ b/libraries/AP_DCM/AP_DCM_HIL.h @@ -31,7 +31,7 @@ public: void set_compass(Compass *compass) {} // Methods - void update_DCM(float _G_Dt) {} + void update_DCM(void) {} float get_health(void) { return 1.0; } From ea26a06f49ca47b6ab5402a2658636b8d703dd1e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:04:53 +1000 Subject: [PATCH 074/100] updates for new DCM code G_Dt is no longer needed by the DCM --- ArduCopter/ArduCopter.pde | 2 +- ArduCopter/test.pde | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0f0a93c438..b5aac40ecb 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1209,7 +1209,7 @@ static void read_AHRS(void) hil.update(); #endif - dcm.update_DCM_fast(G_Dt);//, _tog); + dcm.update_DCM_fast(); omega = dcm.get_gyro(); } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index ab6a17085f..94144ff68c 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -362,7 +362,7 @@ test_adc(uint8_t argc, const Menu::arg *argv) while(1){ for(int i = 0; i < 9; i++){ - Serial.printf_P(PSTR("%d,"),adc.Ch(i)); + Serial.printf_P(PSTR("%u,"),adc.Ch(i)); } Serial.println(); delay(20); From 4fed88ffbff012d85f19dda8424cc9d33eb3f989 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:05:31 +1000 Subject: [PATCH 075/100] ArduPilot updates for new DCM code G_Dt is no longer needed, and scale ADC values by 8 to match old constants --- ArduPlane/ArduPlane.pde | 2 +- ArduPlane/test.pde | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 1c71b6b5be..569708c354 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -502,7 +502,7 @@ static void fast_loop() hil.update(); #endif - dcm.update_DCM(G_Dt); + dcm.update_DCM(); // uses the yaw from the DCM to give more accurate turns calc_bearing_error(); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 926e95ca1a..3b95d9c6c0 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -418,7 +418,7 @@ test_adc(uint8_t argc, const Menu::arg *argv) delay(1000); while(1){ - for (int i=0;i<9;i++) Serial.printf_P(PSTR("%d\t"),adc.Ch(i)); + for (int i=0;i<9;i++) Serial.printf_P(PSTR("%u\t"),adc.Ch(i)); Serial.println(); delay(100); if(Serial.available() > 0){ @@ -476,7 +476,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) // IMU // --- - dcm.update_DCM(G_Dt); + dcm.update_DCM(); if(g.compass_enabled) { medium_loopCounter++; @@ -563,7 +563,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) // IMU // --- - dcm.update_DCM(G_Dt); + dcm.update_DCM(); medium_loopCounter++; if(medium_loopCounter == 5){ From 55bcb8a1c0e5a24b42c611bc4843d353f7bb27f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 13:06:04 +1000 Subject: [PATCH 076/100] RangeFinder: use new ADC interface the new interface gives values 8 times as large as previously --- libraries/AP_RangeFinder/RangeFinder.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 4621a90be8..5a53183c17 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -37,8 +37,9 @@ void RangeFinder::set_orientation(int x, int y, int z) int RangeFinder::read() { // read from the analog port or pitot tube - if( _ap_adc != NULL ){ - raw_value = _ap_adc->Ch_raw(AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL) >> 2; // values from ADC are twice as big as you'd expect + if( _ap_adc != NULL ){ + // values from ADC are twice as big as you'd expect + raw_value = _ap_adc->Ch(AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL) >> 2; }else{ // read raw sensor value and convert to distance raw_value = analogRead(_analogPort); From 1c5e8f0381916a203050a7ec6d4aa950605ced07 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 18:45:51 +1000 Subject: [PATCH 077/100] change ADC sample rate to 1kHz this costs us about 9% of our CPU, but should make aliasing much less of a problem. --- libraries/AP_ADC/AP_ADC.h | 5 +- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 134 +++++++++++++++------------- libraries/AP_ADC/AP_ADC_ADS7844.h | 8 +- libraries/AP_ADC/AP_ADC_HIL.cpp | 6 +- libraries/AP_ADC/AP_ADC_HIL.h | 10 +-- libraries/AP_IMU/AP_IMU_Oilpan.cpp | 2 +- libraries/AP_IMU/IMU.h | 8 +- 7 files changed, 90 insertions(+), 83 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index cafc460c1a..5d749c9ae9 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -33,10 +33,11 @@ class AP_ADC Pass in an array of 6 channel numbers and results are returned in result[] - The function returns the amount of time that the returned - value has been averaged over, in 2.5 millisecond units + The function returns the amount of time (in microseconds) + since the last call to Ch6(). */ virtual uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result) = 0; + private: }; diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index 1625809755..ec9959f407 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -19,9 +19,11 @@ XCK2 = SCK = pin PH2 Chip Select pin is PC4 (33) [PH6 (9)] We are using the 16 clocks per conversion timming to increase efficiency (fast) - The sampling frequency is 400Hz (Timer2 overflow interrupt) + + The sampling frequency is 1kHz (Timer2 overflow interrupt) + So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so - we have an 4x oversampling and averaging. + we have an 10x oversampling and averaging. Methods: Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt) @@ -60,14 +62,14 @@ static volatile uint32_t _sum[8]; // how many values we've accumulated since last read static volatile uint16_t _count[8]; - -static unsigned char ADC_SPI_transfer(unsigned char data) -{ - /* Wait for empty transmit buffer */ - while ( !( UCSR2A & (1 << UDRE2)) ); - /* Put data into buffer, sends the data */ - UDR2 = data; - /* Wait for data to be received */ + +static uint32_t last_ch6_micros; + +static inline unsigned char ADC_SPI_transfer(unsigned char data) +{ + /* Put data into buffer, sends the data */ + UDR2 = data; + /* Wait for data to be received */ while ( !(UCSR2A & (1 << RXC2)) ); /* Get and return received data from buffer */ return UDR2; @@ -75,38 +77,39 @@ static unsigned char ADC_SPI_transfer(unsigned char data) ISR (TIMER2_OVF_vect) -{ - uint8_t ch; - - //bit_set(PORTL,6); // To test performance - +{ + uint8_t ch; + bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel - - for (ch = 0; ch < 8; ch++){ - uint16_t adc_tmp; - adc_tmp = ADC_SPI_transfer(0) << 8; // Read first byte - adc_tmp |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command - - _count[ch]++; - if (_count[ch] == 0) { + + for (ch = 0; ch < 8; ch++) { + uint16_t count = _count[ch]; + uint32_t sum = _sum[ch]; + + if (++count == 0) { // overflow ... shouldn't happen too often // unless we're just not using the // channel. Notice that we overflow the count // to 1 here, not zero, as otherwise the // reader below could get a division by zero - _sum[ch] = 0; - _count[ch] = 1; + sum = 0; + count = 1; + last_ch6_micros = micros(); } - _sum[ch] += adc_tmp; - } - + _count[ch] = count; + + sum += ADC_SPI_transfer(0) << 8; // Read first byte + sum += ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command + + _sum[ch] = sum; + } + bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) - //bit_clear(PORTL,6); // To test performance - TCNT2 = 104; // 400Hz interrupt -} - - + TCNT2 = 200; +} + + // Constructors //////////////////////////////////////////////////////////////// AP_ADC_ADS7844::AP_ADC_ADS7844() { @@ -114,21 +117,21 @@ AP_ADC_ADS7844::AP_ADC_ADS7844() // Public Methods ////////////////////////////////////////////////////////////// void AP_ADC_ADS7844::Init(void) -{ - pinMode(ADC_CHIP_SELECT, OUTPUT); - - digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low) - - // Setup Serial Port2 in SPI mode - UBRR2 = 0; - DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode - // Set MSPI mode of operation and SPI data mode 0. - UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2); - // Enable receiver and transmitter. - UCSR2B = (1 << RXEN2) | (1 << TXEN2); - // Set Baud rate - UBRR2 = 2; // SPI clock running at 2.6MHz - +{ + pinMode(ADC_CHIP_SELECT, OUTPUT); + + digitalWrite(ADC_CHIP_SELECT, HIGH); // Disable device (Chip select is active low) + + // Setup Serial Port2 in SPI mode + UBRR2 = 0; + DDRH |= (1 << PH2); // SPI clock XCK2 (PH2) as output. This enable SPI Master mode + // Set MSPI mode of operation and SPI data mode 0. + UCSR2C = (1 << UMSEL21) | (1 << UMSEL20); // |(0 << UCPHA2) | (0 << UCPOL2); + // Enable receiver and transmitter. + UCSR2B = (1 << RXEN2) | (1 << TXEN2); + // Set Baud rate + UBRR2 = 2; // SPI clock running at 2.6MHz + // get an initial value for each channel. This ensures // _count[] is never zero for (uint8_t i=0; i<8; i++) { @@ -139,17 +142,18 @@ void AP_ADC_ADS7844::Init(void) _sum[i] = adc_tmp; } - - // Enable Timer2 Overflow interrupt to capture ADC data - TIMSK2 = 0; // Disable interrupts - TCCR2A = 0; // normal counting mode - TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256 - TCNT2 = 0; - TIFR2 = _BV(TOV2); // clear pending interrupts; - TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt -} - -// Read one channel value + last_ch6_micros = micros(); + + // Enable Timer2 Overflow interrupt to capture ADC data + TIMSK2 = 0; // Disable interrupts + TCCR2A = 0; // normal counting mode + TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256 + TCNT2 = 0; + TIFR2 = _BV(TOV2); // clear pending interrupts; + TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt +} + +// Read one channel value uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) { uint16_t count; @@ -174,7 +178,7 @@ uint16_t AP_ADC_ADS7844::Ch(uint8_t ch_num) // equal. This will only be true if we always consistently access a // sensor by either Ch6() or Ch() and never mix them. If you mix them // then you will get very strange results -uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) +uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) { uint16_t count[6]; uint32_t sum[6]; @@ -203,7 +207,9 @@ uint16_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, uint16_t *result) result[i] = sum[i] / count[i]; } - // this assumes the count in all channels is equal, which will - // be true if the callers are using the interface consistently - return count[0]; -} + // return number of microseconds since last call + uint32_t us = micros(); + uint32_t ret = us - last_ch6_micros; + last_ch6_micros = us; + return ret; +} diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index c4b99ecaed..35f72d36db 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -24,9 +24,9 @@ class AP_ADC_ADS7844 : public AP_ADC uint16_t Ch(unsigned char ch_num); // Read 6 sensors at once - uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result); - - private: -}; + uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + + private: +}; #endif diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp index 1684b161b7..108bb1120f 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ b/libraries/AP_ADC/AP_ADC_HIL.cpp @@ -47,11 +47,11 @@ void AP_ADC_HIL::Init(void) uint16_t AP_ADC_HIL::Ch(unsigned char ch_num) { return adcValue[ch_num]; -} +} // Read 6 channel values -uint16_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) -{ +uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, uint16_t *result) +{ for (uint8_t i=0; i<6; i++) { result[i] = Ch(channel_numbers[i]); } diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index ed0c039289..1794da8eaf 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -34,12 +34,12 @@ class AP_ADC_HIL : public AP_ADC // Read the sensor, part of public AP_ADC interface uint16_t Ch(unsigned char ch_num); - /// + /// // Read 6 sensors at once - uint16_t Ch6(const uint8_t *channel_numbers, uint16_t *result); - - /// - // Set the adc raw values given the current rotations rates, + uint32_t Ch6(const uint8_t *channel_numbers, uint16_t *result); + + /// + // Set the adc raw values given the current rotations rates, // temps, accels, and pressures void setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp, int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress); diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index b332142520..73cef75e4e 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -279,7 +279,7 @@ AP_IMU_Oilpan::update(void) int tc_temp = _adc->Ch(_gyro_temp_ch); uint16_t adc_values[6]; - _ticks = _adc->Ch6(_sensors, adc_values); + _sample_time = _adc->Ch6(_sensors, adc_values); // convert corrected gyro readings to delta acceleration // diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index 42a128ea58..1b881bc355 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -81,7 +81,7 @@ public: /// /// @returns number of seconds /// - float get_delta_time(void) { return _ticks * (2.5/1000.0); } + float get_delta_time(void) { return _sample_time * 1.0e-6; } /// A count of bad sensor readings /// @@ -98,9 +98,9 @@ protected: /// Most recent gyro reading obtained by ::update Vector3f _gyro; - /// number of 2.5ms ticks that the accel and gyro values - /// were calculated from - uint16_t _ticks; + /// number of microseconds that the accel and gyro values + /// were sampled over + uint32_t _sample_time; }; #endif From 812cd3562fd2bb365a25b6b8f322365bdbf5a50e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 18:46:47 +1000 Subject: [PATCH 078/100] updated ADC test for new API --- .../examples/AP_ADC_test/AP_ADC_test.pde | 53 ++++++++++++------- 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index ba4b74baea..e0b02ff119 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -1,37 +1,52 @@ + /* Example of APM_ADC library. Code by Jordi Muñoz and Jose Julio. DIYDrones.com */ +#include #include // ArduPilot Mega ADC Library -unsigned long timer; +FastSerialPort0(Serial); // FTDI/console +unsigned long timer; AP_ADC_ADS7844 adc; void setup() { - adc.Init(); // APM ADC initialization - Serial.begin(57600); - Serial.println("ArduPilot Mega ADC library test"); - delay(1000); - timer = millis(); + Serial.begin(115200, 128, 128); + Serial.println("ArduPilot Mega ADC library test"); + delay(1000); + adc.Init(); // APM ADC initialization + delay(1000); + timer = millis(); } +static const uint8_t channel_map[6] = { 1, 2, 0, 4, 5, 6}; +static uint16_t sin_count; +float v; +uint32_t last_usec = 0; + void loop() { - int ch; - - if((millis()- timer) > 20) - { - timer = millis(); - for (ch=0;ch<7;ch++) - { - Serial.print(adc.Ch(ch)); - Serial.print(","); - } - Serial.println(); - } + v = sin(millis()); + sin_count++; + + if ((millis() - timer) > 200) { + uint16_t result[6]; + uint32_t deltat; + uint16_t ch3; + + timer = millis(); + + ch3 = adc.Ch(3); + deltat = adc.Ch6(channel_map, result); + + Serial.printf("gx=%u gy=%u gz=%u ax=%u ay=%u az=%u gt=%u deltat=%lu sin_count=%u\n", + result[0], result[1], result[2], + result[3], result[4], result[5], + ch3, deltat, sin_count); + sin_count = 0; + } } -// end of test program From 2ae78e197e4a899b7ba9052bb68870051f720c38 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 23:03:24 +1000 Subject: [PATCH 079/100] ADC: the bottom 3 bits of ADC output are always zero its a 12 bit ADC .... silly of me to think the bottom 3 bits are valid! --- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index ec9959f407..ce4fabe70b 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -84,25 +84,29 @@ ISR (TIMER2_OVF_vect) ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel for (ch = 0; ch < 8; ch++) { - uint16_t count = _count[ch]; - uint32_t sum = _sum[ch]; + uint16_t v; - if (++count == 0) { + v = ADC_SPI_transfer(0) << 8; // Read first byte + v |= ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command + + if (v & 0x8007) { + // this is a 12-bit ADC, shifted by 3 bits. + // if we get other bits set then the value is + // bogus and should be ignored + continue; + } + + if (++_count[ch] == 0) { // overflow ... shouldn't happen too often // unless we're just not using the // channel. Notice that we overflow the count // to 1 here, not zero, as otherwise the // reader below could get a division by zero - sum = 0; - count = 1; + _sum[ch] = 0; + _count[ch] = 1; last_ch6_micros = micros(); } - _count[ch] = count; - - sum += ADC_SPI_transfer(0) << 8; // Read first byte - sum += ADC_SPI_transfer(adc_cmd[ch + 1]); // Read second byte and send next command - - _sum[ch] = sum; + _sum[ch] += (v >> 3); } bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) From 4a7c9c406f3114537e071cd660c7be3955b5dbc3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Sep 2011 23:06:21 +1000 Subject: [PATCH 080/100] show noise levels in ADC test --- .../examples/AP_ADC_test/AP_ADC_test.pde | 59 +++++++++++-------- 1 file changed, 35 insertions(+), 24 deletions(-) diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index e0b02ff119..8813fd669d 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -20,33 +20,44 @@ void setup() adc.Init(); // APM ADC initialization delay(1000); timer = millis(); -} - +} + static const uint8_t channel_map[6] = { 1, 2, 0, 4, 5, 6}; -static uint16_t sin_count; float v; uint32_t last_usec = 0; -void loop() -{ - v = sin(millis()); - sin_count++; +void loop() +{ + uint16_t result[6]; + uint32_t deltat = 0; + uint16_t ch3; + uint16_t min[6], max[6]; + uint8_t i; - if ((millis() - timer) > 200) { - uint16_t result[6]; - uint32_t deltat; - uint16_t ch3; - - timer = millis(); - - ch3 = adc.Ch(3); - deltat = adc.Ch6(channel_map, result); - - Serial.printf("gx=%u gy=%u gz=%u ax=%u ay=%u az=%u gt=%u deltat=%lu sin_count=%u\n", - result[0], result[1], result[2], - result[3], result[4], result[5], - ch3, deltat, sin_count); - sin_count = 0; + for (i=0;i<6;i++) { + min[i] = 0xFFFF; + max[i] = 0; } -} - + + + do { + ch3 = adc.Ch(3); + deltat += adc.Ch6(channel_map, result); + for (i=0;i<6;i++) { + if (result[i] < min[i]) min[i] = result[i]; + if (result[i] > max[i]) max[i] = result[i]; + if (result[i] & 0x8000) { + Serial.printf("result[%u]=0x%04x\n", (unsigned)i, result[i]); + } + } + } while ((millis() - timer) < 200); + + timer = millis(); + Serial.printf("g=(%u,%u,%u) a=(%u,%u,%u) +/-(%u,%u,%u,%u,%u,%u) gt=%u dt=%u\n", + result[0], result[1], result[2], + result[3], result[4], result[5], + (max[0]-min[0]), (max[1]-min[1]), (max[2]-min[2]), + (max[3]-min[3]), (max[4]-min[4]), (max[5]-min[5]), + ch3, (unsigned)(deltat/1000)); +} + From 1b5c7e8a9054fa09dbd974ba67a2863302285ab9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 16 Sep 2011 16:52:29 +1000 Subject: [PATCH 081/100] ADC: show timing information in ADC test --- .../examples/AP_ADC_test/AP_ADC_test.pde | 33 ++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index 8813fd669d..93069bf928 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -26,7 +26,29 @@ static const uint8_t channel_map[6] = { 1, 2, 0, 4, 5, 6}; float v; uint32_t last_usec = 0; -void loop() +static void show_timing() +{ + uint32_t mint = (uint32_t)-1, maxt = 0, totalt=0; + uint32_t start_time = millis(); + uint16_t result[6]; + uint32_t count = 0; + + Serial.println("Starting timing test"); + + adc.Ch6(channel_map, result); + + do { + uint32_t deltat = adc.Ch6(channel_map, result); + if (deltat > maxt) maxt = deltat; + if (deltat < mint) mint = deltat; + totalt += deltat; + count++; + } while ((millis() - start_time) < 5000); + + Serial.printf("timing: mint=%lu maxt=%lu avg=%lu\n", mint, maxt, totalt/count); +} + +static void show_data() { uint16_t result[6]; uint32_t deltat = 0; @@ -61,3 +83,12 @@ void loop() ch3, (unsigned)(deltat/1000)); } + +void loop() +{ + if (millis() < 5000) { + show_timing(); + } else { + show_data(); + } +} From 1f59890cde820629b7ac059672e82e29e7128639 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2011 12:15:02 +1000 Subject: [PATCH 082/100] Tools: added CPUInfo sketch this shows the approximate cost of a wide range of common operations, and the sizes of most data types --- Tools/CPUInfo/CPUInfo.pde | 122 ++++++++++++++++++++++++++++++++++++++ Tools/CPUInfo/Makefile | 4 ++ 2 files changed, 126 insertions(+) create mode 100644 Tools/CPUInfo/CPUInfo.pde create mode 100644 Tools/CPUInfo/Makefile diff --git a/Tools/CPUInfo/CPUInfo.pde b/Tools/CPUInfo/CPUInfo.pde new file mode 100644 index 0000000000..4083ca57e1 --- /dev/null +++ b/Tools/CPUInfo/CPUInfo.pde @@ -0,0 +1,122 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/* + test CPU speed + Andrew Tridgell September 2011 +*/ + +#include +#include + +FastSerialPort0(Serial); + +#define SERIAL0_BAUD 115200 + +void setup() { + Serial.begin(SERIAL0_BAUD, 128, 128); +} + +static void show_sizes(void) +{ + Serial.println("Type sizes:"); + Serial.printf("char : %d\n", sizeof(char)); + Serial.printf("short : %d\n", sizeof(short)); + Serial.printf("int : %d\n", sizeof(int)); + Serial.printf("long : %d\n", sizeof(long)); + Serial.printf("long long : %d\n", sizeof(long long)); + Serial.printf("bool : %d\n", sizeof(bool)); + Serial.printf("void* : %d\n", sizeof(void *)); +} + +#define TENTIMES(x) do { x; x; x; x; x; x; x; x; x; x; } while (0) +#define FIFTYTIMES(x) do { TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); TENTIMES(x); } while (0) + +#define TIMEIT(name, op, count) do { \ + uint32_t us_end, us_start; \ + us_start = micros(); \ + for (uint8_t i=0; i Date: Sat, 17 Sep 2011 12:15:41 +1000 Subject: [PATCH 083/100] build: cope with more depths for libraries this finds the libraries at more levels up from the current directory --- libraries/AP_Common/Arduino.mk | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index be6928f17c..3d19e12593 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -60,6 +60,12 @@ endif # ifeq ($(SKETCHBOOK),) SKETCHBOOK := $(shell cd $(SRCROOT)/.. && pwd) + ifeq ($(wildcard $(SKETCHBOOK)/libraries),) + SKETCHBOOK := $(shell cd $(SRCROOT)/../.. && pwd) + endif + ifeq ($(wildcard $(SKETCHBOOK)/libraries),) + SKETCHBOOK := $(shell cd $(SRCROOT)/../../.. && pwd) + endif ifeq ($(wildcard $(SKETCHBOOK)/libraries),) SKETCHBOOK := $(shell cd $(SRCROOT)/../../../.. && pwd) endif From a828d00125f0d8c9426c1c13d0d3ac6ee7ed9b16 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2011 12:30:19 +1000 Subject: [PATCH 084/100] ADC: randomise the sampling rate this is based on suggestions from Kari and Mike, and should reduce the aliasing effects we get from the simple averaging --- libraries/AP_ADC/AP_ADC_ADS7844.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.cpp b/libraries/AP_ADC/AP_ADC_ADS7844.cpp index ce4fabe70b..be90bba00a 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS7844.cpp @@ -65,6 +65,14 @@ static volatile uint16_t _count[8]; static uint32_t last_ch6_micros; +// TCNT2 values for various interrupt rates, +// assuming 256 prescaler. Note that these values +// assume a zero-time ISR. The actual rate will be a +// bit lower than this +#define TCNT2_781_HZ (256-80) +#define TCNT2_1008_HZ (256-62) +#define TCNT2_1302_HZ (256-48) + static inline unsigned char ADC_SPI_transfer(unsigned char data) { /* Put data into buffer, sends the data */ @@ -76,9 +84,10 @@ static inline unsigned char ADC_SPI_transfer(unsigned char data) } -ISR (TIMER2_OVF_vect) +ISR (TIMER2_OVF_vect) { uint8_t ch; + static uint8_t timer_offset; bit_clear(PORTC, 4); // Enable Chip Select (PIN PC4) ADC_SPI_transfer(adc_cmd[0]); // Command to read the first channel @@ -110,7 +119,11 @@ ISR (TIMER2_OVF_vect) } bit_set(PORTC, 4); // Disable Chip Select (PIN PC4) - TCNT2 = 200; + + // this gives us a sample rate between 781Hz and 1302Hz. We + // randomise it to try to minimise aliasing effects + timer_offset = (timer_offset + 49) % 32; + TCNT2 = TCNT2_781_HZ + timer_offset; } @@ -151,10 +164,10 @@ void AP_ADC_ADS7844::Init(void) // Enable Timer2 Overflow interrupt to capture ADC data TIMSK2 = 0; // Disable interrupts TCCR2A = 0; // normal counting mode - TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of 256 + TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256 TCNT2 = 0; TIFR2 = _BV(TOV2); // clear pending interrupts; - TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt + TIMSK2 = _BV(TOIE2); // enable the overflow interrupt } // Read one channel value From a13c68a404d022ceccf7d708f7184ac381f6791b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2011 13:38:18 +1000 Subject: [PATCH 085/100] fixed build of OpticalFlow code --- ArduCopter/ArduCopter.pde | 2 +- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 10 ++++++++- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 4 +++- .../AP_OpticalFlow_ADNS3080.cpp | 22 +++++++++---------- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 5 ++--- 5 files changed, 26 insertions(+), 17 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b5aac40ecb..e6d3defd2c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -61,7 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List #include // PI library #include // RC Channel Library #include // Range finder library -#include // Optical Flow library +#include // Optical Flow library #include #include // MAVLink GCS definitions #include diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 2613acae3e..da47395701 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -31,12 +31,14 @@ AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix) // read latest values from sensor and fill in x,y and totals int AP_OpticalFlow::read() { + return 0; } // reads a value from the sensor (will be sensor specific) byte AP_OpticalFlow::read_register(byte address) { + return 0; } // writes a value to one of the sensor's register (will be sensor specific) @@ -89,6 +91,8 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float change_x = dx - exp_change_x; change_y = dy - exp_change_y; + float avg_altitude = (altitude + _last_altitude)*0.5; + // convert raw change to horizontal movement in cm x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer? y_cm = -change_y * avg_altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less @@ -97,6 +101,10 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float vlon = x_cm * sin_yaw_y - y_cm * cos_yaw_x; vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; } + + _last_altitude = altitude; + _last_roll = roll; + _last_pitch = pitch; } @@ -124,4 +132,4 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float } } -*/ \ No newline at end of file +*/ diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 79f1bb215f..7d57977174 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -67,9 +67,11 @@ class AP_OpticalFlow Matrix3f _orientation_matrix; float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) float radians_to_pixels; - float _last_roll, _last_pitch, _last_yaw, _last_altitude; + float _last_roll, _last_pitch, _last_altitude; virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values virtual void update_conversion_factors(); }; +#include "AP_OpticalFlow_ADNS3080.h" + #endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index f42d1acb9c..b387e9ed91 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -73,10 +73,10 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) if( retry < 3 ) { if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) return true; - else - retry++; - }else - return false; + retry++; + } + + return false; } // @@ -218,7 +218,7 @@ void AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn ) { byte regVal = read_register(ADNS3080_CONFIGURATION_BITS); - regVal = regVal & 0xBf | (alwaysOn << 6); + regVal = (regVal & 0xbf) | (alwaysOn << 6); delayMicroseconds(50); // small delay write_register(ADNS3080_CONFIGURATION_BITS, regVal); } @@ -254,7 +254,7 @@ bool AP_OpticalFlow_ADNS3080::get_frame_rate_auto() { byte regVal = read_register(ADNS3080_EXTENDED_CONFIG); - if( regVal & 0x01 > 0 ) { + if( (regVal & 0x01) != 0 ) { return false; }else{ return true; @@ -355,10 +355,10 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed) delayMicroseconds(50); // small delay // determine value to put into extended config - regVal = regVal & ~0x02; + regVal &= ~0x02; }else{ // determine value to put into extended config - regVal = regVal & ~0x02 | 0x02; + regVal |= 0x02; } write_register(ADNS3080_EXTENDED_CONFIG, regVal); delayMicroseconds(50); // small delay @@ -377,7 +377,7 @@ AP_OpticalFlow_ADNS3080::get_shutter_speed() // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) -unsigned int +void AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed) { NumericIntType aNum; @@ -420,7 +420,7 @@ AP_OpticalFlow_ADNS3080::clear_motion() } // get_pixel_data - captures an image from the sensor and stores it to the pixe_data array -int +void AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) { int i,j; @@ -438,7 +438,7 @@ AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort) for( i=0; iprintln("failed to find first pixel"); } isFirstPixel = false; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index e5c296f186..cdb6a2e975 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -4,7 +4,6 @@ //#include //#include //#include -//#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h" // orientations for ADNS3080 sensor @@ -131,11 +130,11 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false) unsigned int get_shutter_speed(); - unsigned int set_shutter_speed(unsigned int shutter_speed); + void set_shutter_speed(unsigned int shutter_speed); void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared - int print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port + void print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port }; #endif From f1974cac21b39f5ae875857f095bc825263876a7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2011 13:42:13 +1000 Subject: [PATCH 086/100] fixed ArduCopter HIL build --- libraries/AP_DCM/AP_DCM_HIL.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h index 071370afa7..a8266fcb09 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ b/libraries/AP_DCM/AP_DCM_HIL.h @@ -32,6 +32,7 @@ public: // Methods void update_DCM(void) {} + void update_DCM_fast(void) {} float get_health(void) { return 1.0; } From fc030db54ecc3d9b98a19bd1426b9e5449b08cda Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2011 14:33:01 +1000 Subject: [PATCH 087/100] fixed bit-logic for simple modes --- ArduCopter/control_modes.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 63c3918534..b31f0b98d9 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -17,7 +17,7 @@ static void read_control_switch() #if CH7_OPTION != SIMPLE_MODE_CONTROL // setup Simple mode // do we enable simple mode? - do_simple = (g.simple_modes & 1 << switchPosition); + do_simple = (g.simple_modes & (1 << switchPosition)); //Serial.printf("do simple: %d \n", (int)do_simple); #endif From 3d28ac834bbc07724210e1b2fcd4ec9db15f0475 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2011 15:06:17 +1000 Subject: [PATCH 088/100] put output of CPUInfo test in git --- Tools/CPUInfo/output.txt | 41 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 Tools/CPUInfo/output.txt diff --git a/Tools/CPUInfo/output.txt b/Tools/CPUInfo/output.txt new file mode 100644 index 0000000000..87d210c77c --- /dev/null +++ b/Tools/CPUInfo/output.txt @@ -0,0 +1,41 @@ +Type sizes: +char : 1 +short : 2 +int : 2 +long : 4 +long long : 8 +bool : 1 +void* : 2 + +Operation timings: +Note: timings for some operations are very data dependent +nop 0.07 usec/call +cli/sei 0.14 usec/call +micros() 3.04 usec/call +millis() 1.34 usec/call +fadd 8.68 usec/call +fsub 8.74 usec/call +fmul 7.21 usec/call +fdiv 7.21 usec/call +dadd 8.68 usec/call +dsub 8.74 usec/call +dmul 5.64 usec/call +ddiv 5.57 usec/call +sin() 114.10 usec/call +cos() 103.03 usec/call +tan() 147.92 usec/call +iadd8 0.48 usec/call +isub8 0.48 usec/call +imul8 0.67 usec/call +idiv8 12.93 usec/call +iadd16 0.92 usec/call +isub16 0.92 usec/call +imul16 1.48 usec/call +idiv16 13.24 usec/call +iadd32 1.80 usec/call +isub32 1.80 usec/call +imul32 4.57 usec/call +idiv32 37.71 usec/call +memcpy128 56.76 usec/call +memset128 49.71 usec/call +delay(1) 1005.14 usec/call From 6fda1e3f14c516ccf3eab0d72af816fa4b9f8587 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 17 Sep 2011 22:09:18 +0900 Subject: [PATCH 089/100] ArduCopter - small changes to fix compile errors when optical flow enabled --- ArduCopter/ArduCopter.pde | 2 +- ArduCopter/Log.pde | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e6d3defd2c..e715de63f7 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -200,7 +200,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1; AP_DCM dcm(&imu, g_gps); #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow(); + AP_OpticalFlow_ADNS3080 optflow; #endif #endif diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 232f60e9dd..98dbf41998 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -503,8 +503,8 @@ static void Log_Write_Optflow() DataFlash.WriteInt((int)optflow.dx); DataFlash.WriteInt((int)optflow.dy); DataFlash.WriteInt((int)optflow.surface_quality); - DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat); - DataFlash.WriteLong(optflow.lon);//optflow_offset.lng + optflow.lng); + DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); + DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); DataFlash.WriteByte(END_BYTE); } #endif From acdd6b8a83bf6df3c2ffebd93f8525e8e031ff95 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 17 Sep 2011 21:22:07 +0800 Subject: [PATCH 090/100] APM Planner 1.0.69 prep for ac2 2.0.43 - simple mode modify some scaling in Config add hud speed warning. add link quality and time to HUD fix ac2 logs, relative alt. prep for mavlink 1.0 add time to tlog > plain text conversion --- .../ArdupilotMegaPlanner/ArdupilotMega.csproj | 1 + Tools/ArdupilotMegaPlanner/Common.cs | 13 +- Tools/ArdupilotMegaPlanner/CurrentState.cs | 19 +- .../GCSViews/Configuration.Designer.cs | 63 +- .../GCSViews/Configuration.resx | 10694 ++++++++-------- .../GCSViews/FlightData.cs | 13 +- .../GCSViews/FlightPlanner.cs | 4 +- Tools/ArdupilotMegaPlanner/HUD.cs | 138 +- Tools/ArdupilotMegaPlanner/Log.cs | 13 +- Tools/ArdupilotMegaPlanner/MAVLink.cs | 220 +- Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs | 1918 +-- .../ArdupilotMegaPlanner/MAVLinkTypesenum.cs | 154 +- Tools/ArdupilotMegaPlanner/MainV2.cs | 28 +- Tools/ArdupilotMegaPlanner/MavlinkLog.cs | 5 +- .../Properties/AssemblyInfo.cs | 2 +- .../Resources/MAVParam.txt | 14 +- .../Setup/Setup.Designer.cs | 402 +- Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 11 + Tools/ArdupilotMegaPlanner/Setup/Setup.resx | 3117 +++-- .../bin/Release/GCSViews/Configuration.resx | 10694 ++++++++-------- .../bin/Release/dataflashlog.xml | 12 +- Tools/ArdupilotMegaPlanner/dataflashlog.xml | 12 +- Tools/ArdupilotMegaPlanner/georefimage.cs | 127 + Tools/ArdupilotMegaPlanner/hudold.cs | 1149 ++ .../mavlinklist-xml-dontuse.pl | 132 + Tools/ArdupilotMegaPlanner/mavlinklist.pl | 133 +- Tools/ArdupilotMegaPlanner/srtm.cs | 161 +- 27 files changed, 16161 insertions(+), 13088 deletions(-) create mode 100644 Tools/ArdupilotMegaPlanner/georefimage.cs create mode 100644 Tools/ArdupilotMegaPlanner/hudold.cs create mode 100644 Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index ddfdd3e8d6..b75096d41e 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -188,6 +188,7 @@ Component + diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index a9c7314de0..ccd82ba138 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -240,13 +240,12 @@ namespace ArdupilotMega { STABILIZE = 0, // hold level position ACRO = 1, // rate control - SIMPLE = 2, // - ALT_HOLD = 3, // AUTO control - AUTO = 4, // AUTO control - GUIDED = 5, // AUTO control - LOITER = 6, // Hold a single location - RTL = 7, // AUTO control - CIRCLE = 8 + ALT_HOLD = 2, // AUTO control + AUTO = 3, // AUTO control + GUIDED = 4, // AUTO control + LOITER = 5, // Hold a single location + RTL = 6, // AUTO control + CIRCLE = 7 } public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode) diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 71db65033b..8cf6af287d 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -65,7 +65,7 @@ namespace ArdupilotMega public float gz { get; set; } // calced turn rate - public float turnrate { get { if (groundspeed == 0) return 0; return (roll * 9.8f) / groundspeed; } } + public float turnrate { get { if (groundspeed <= 1) return 0; return (roll * 9.8f) / groundspeed; } } //radio public float ch1in { get; set; } @@ -101,17 +101,17 @@ namespace ArdupilotMega //nav state public float nav_roll { get; set; } public float nav_pitch { get; set; } - public short nav_bearing { get; set; } - public short target_bearing { get; set; } - public ushort wp_dist { get { return (ushort)(_wpdist * multiplierdist); } set { _wpdist = value; } } + public float nav_bearing { get; set; } + public float target_bearing { get; set; } + public float wp_dist { get { return (_wpdist * multiplierdist); } set { _wpdist = value; } } public float alt_error { get { return _alt_error * multiplierdist; } set { _alt_error = value; } } public float ber_error { get { return (target_bearing - yaw); } set { } } public float aspd_error { get { return _aspd_error * multiplierspeed; } set { _aspd_error = value; } } public float xtrack_error { get; set; } - public int wpno { get; set; } + public float wpno { get; set; } public string mode { get; set; } public float climbrate { get; set; } - ushort _wpdist; + float _wpdist; float _aspd_error; float _alt_error; @@ -150,7 +150,8 @@ namespace ArdupilotMega public float brklevel { get; set; } // stats - public ushort packetdrop { get; set; } + public ushort packetdropremote { get; set; } + public ushort linkqualitygcs { get; set; } // requested stream rates public byte rateattitude { get; set; } @@ -361,7 +362,7 @@ namespace ArdupilotMega battery_voltage = sysstatus.vbat; battery_remaining = sysstatus.battery_remaining; - packetdrop = sysstatus.packet_drop; + packetdropremote = sysstatus.packet_drop; if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { @@ -468,7 +469,7 @@ namespace ArdupilotMega wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp); - int oldwp = wpno; + int oldwp = (int)wpno; wpno = wpcur.seq; diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs index 82d0c1de51..a93b411f51 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs @@ -30,8 +30,8 @@ { this.components = new System.ComponentModel.Container(); System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration)); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle(); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); this.Params = new System.Windows.Forms.DataGridView(); this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn(); this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn(); @@ -167,9 +167,6 @@ this.label21 = new System.Windows.Forms.Label(); this.THR_HOLD_P = new System.Windows.Forms.DomainUpDown(); this.label22 = new System.Windows.Forms.Label(); - this.groupBox18 = new System.Windows.Forms.GroupBox(); - this.PITCH_MAX = new System.Windows.Forms.DomainUpDown(); - this.label27 = new System.Windows.Forms.Label(); this.groupBox19 = new System.Windows.Forms.GroupBox(); this.HLD_LAT_IMAX = new System.Windows.Forms.DomainUpDown(); this.label28 = new System.Windows.Forms.Label(); @@ -285,7 +282,6 @@ this.groupBox4.SuspendLayout(); this.groupBox6.SuspendLayout(); this.groupBox7.SuspendLayout(); - this.groupBox18.SuspendLayout(); this.groupBox19.SuspendLayout(); this.groupBox20.SuspendLayout(); this.groupBox21.SuspendLayout(); @@ -302,14 +298,14 @@ this.Params.AllowUserToAddRows = false; this.Params.AllowUserToDeleteRows = false; resources.ApplyResources(this.Params, "Params"); - dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon; - dataGridViewCellStyle3.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White; - dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3; + dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon; + dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White; + dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1; this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize; this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] { this.Command, @@ -318,14 +314,14 @@ this.mavScale, this.RawValue}); this.Params.Name = "Params"; - dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption; - dataGridViewCellStyle4.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText; - dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4; + dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption; + dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText; + dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2; this.Params.RowHeadersVisible = false; this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged); // @@ -990,7 +986,6 @@ this.TabAC2.Controls.Add(this.groupBox4); this.TabAC2.Controls.Add(this.groupBox6); this.TabAC2.Controls.Add(this.groupBox7); - this.TabAC2.Controls.Add(this.groupBox18); this.TabAC2.Controls.Add(this.groupBox19); this.TabAC2.Controls.Add(this.groupBox20); this.TabAC2.Controls.Add(this.groupBox21); @@ -1157,24 +1152,6 @@ resources.ApplyResources(this.label22, "label22"); this.label22.Name = "label22"; // - // groupBox18 - // - this.groupBox18.Controls.Add(this.PITCH_MAX); - this.groupBox18.Controls.Add(this.label27); - resources.ApplyResources(this.groupBox18, "groupBox18"); - this.groupBox18.Name = "groupBox18"; - this.groupBox18.TabStop = false; - // - // PITCH_MAX - // - resources.ApplyResources(this.PITCH_MAX, "PITCH_MAX"); - this.PITCH_MAX.Name = "PITCH_MAX"; - // - // label27 - // - resources.ApplyResources(this.label27, "label27"); - this.label27.Name = "label27"; - // // groupBox19 // this.groupBox19.Controls.Add(this.HLD_LAT_IMAX); @@ -1887,7 +1864,6 @@ this.groupBox4.ResumeLayout(false); this.groupBox6.ResumeLayout(false); this.groupBox7.ResumeLayout(false); - this.groupBox18.ResumeLayout(false); this.groupBox19.ResumeLayout(false); this.groupBox20.ResumeLayout(false); this.groupBox21.ResumeLayout(false); @@ -2031,9 +2007,6 @@ private System.Windows.Forms.Label label21; private System.Windows.Forms.DomainUpDown THR_HOLD_P; private System.Windows.Forms.Label label22; - private System.Windows.Forms.GroupBox groupBox18; - private System.Windows.Forms.DomainUpDown PITCH_MAX; - private System.Windows.Forms.Label label27; private System.Windows.Forms.GroupBox groupBox19; private System.Windows.Forms.DomainUpDown HLD_LAT_IMAX; private System.Windows.Forms.Label label28; diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx index 8c4784c6d9..1ecdd308f8 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx @@ -192,6 +192,2421 @@ Top, Bottom, Left, Right + + 111, 82 + + + 78, 20 + + + 11 + + + THR_FS_VALUE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 0 + + + NoControl + + + 6, 86 + + + 50, 13 + + + 12 + + + FS Value + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + THR_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 2 + + + NoControl + + + 6, 63 + + + 27, 13 + + + 13 + + + Max + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + THR_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 4 + + + NoControl + + + 6, 40 + + + 24, 13 + + + 14 + + + Min + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_THROTTLE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 6 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 15 + + + Cruise + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 7 + + + 405, 217 + + + 195, 108 + + + 0 + + + Throttle 0-100% + + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 0 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSPD_RATIO + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 0 + + + NoControl + + + 6, 87 + + + 32, 13 + + + 1 + + + Ratio + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSPD_FBW_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 2 + + + NoControl + + + 6, 59 + + + 53, 13 + + + 3 + + + FBW max + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSPD_FBW_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 4 + + + NoControl + + + 6, 40 + + + 50, 13 + + + 5 + + + FBW min + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_ARSPD_CM + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 6 + + + NoControl + + + 6, 17 + + + 64, 13 + + + 6 + + + Cruise + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 7 + + + 406, 325 + + + 195, 108 + + + 1 + + + Airspeed m/s + + + groupBox1 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + LIM_PITCH_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 0 + + + NoControl + + + 6, 63 + + + 51, 13 + + + 10 + + + Pitch Min + + + label39 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 1 + + + 111, 36 + + + 78, 20 + + + 7 + + + LIM_PITCH_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 2 + + + NoControl + + + 6, 40 + + + 54, 13 + + + 11 + + + Pitch Max + + + label38 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 5 + + + LIM_ROLL_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 4 + + + NoControl + + + 6, 17 + + + 55, 13 + + + 12 + + + Bank Max + + + label37 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 5 + + + 205, 325 + + + 195, 108 + + + 2 + + + Navigation Angles + + + groupBox2 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 2 + + + 111, 36 + + + 78, 20 + + + 7 + + + XTRK_ANGLE_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 0 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 8 + + + Entry Angle + + + label79 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 1 + + + 111, 13 + + + 78, 20 + + + 5 + + + XTRK_GAIN_SC + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 2 + + + NoControl + + + 6, 17 + + + 52, 13 + + + 9 + + + Gain (cm) + + + label80 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 3 + + + 4, 325 + + + 195, 108 + + + 3 + + + Xtrack Pids + + + groupBox15 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 13 + + + KFF_PTCH2THR + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 0 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 14 + + + P to T + + + label83 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + KFF_RDDRMIX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 2 + + + NoControl + + + 6, 63 + + + 61, 13 + + + 15 + + + Rudder Mix + + + label78 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + KFF_PTCHCOMP + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 4 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 16 + + + Pitch Comp + + + label81 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 5 + + + 205, 217 + + + 195, 108 + + + 4 + + + Other Mix's + + + groupBox16 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 4 + + + 111, 82 + + + 78, 20 + + + 11 + + + ENRGY2THR_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label73 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + ENRGY2THR_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label74 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + ENRGY2THR_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label75 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + ENRGY2THR_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label76 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 7 + + + 4, 217 + + + 195, 108 + + + 5 + + + Energy/Alt Pid + + + groupBox14 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 5 + + + 111, 82 + + + 78, 20 + + + 0 + + + ALT2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label69 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ALT2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label70 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ALT2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label71 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ALT2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label72 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 7 + + + 406, 109 + + + 195, 108 + + + 6 + + + Nav Pitch Alt Pid + + + groupBox13 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 6 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSP2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label65 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSP2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label66 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSP2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label67 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ARSP2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label68 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 7 + + + 205, 109 + + + 195, 108 + + + 7 + + + Nav Pitch AS Pid + + + groupBox12 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 7 + + + 111, 82 + + + 78, 20 + + + 11 + + + HDNG2RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label61 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + HDNG2RLL_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label62 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + HDNG2RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label63 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + HDNG2RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label64 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 7 + + + 4, 109 + + + 195, 108 + + + 8 + + + Nav Roll Pid + + + groupBox11 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 8 + + + 111, 82 + + + 78, 20 + + + 11 + + + YW2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label57 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + YW2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label58 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + YW2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label59 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + YW2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label60 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 7 + + + 406, 1 + + + 195, 108 + + + 9 + + + Servo Yaw Pid + + + groupBox10 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 9 + + + 111, 82 + + + 78, 20 + + + 11 + + + PTCH2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label53 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + PTCH2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label54 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + PTCH2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label55 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + PTCH2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label56 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 7 + + + 205, 1 + + + 195, 108 + + + 10 + + + Servo Pitch Pid + + + groupBox9 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 10 + + + 111, 82 + + + 78, 20 + + + 11 + + + RLL2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label49 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + RLL2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label50 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + RLL2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label51 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + RLL2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label52 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 7 + + + 4, 1 + + + 195, 108 + + + 11 + + + Servo Roll Pid + + + groupBox8 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 11 + + + 4, 22 + + + 0, 0, 0, 0 + + + 722, 434 + + + 0 + + + APM 2.x + TabAPM2 @@ -204,6 +2619,1824 @@ 0 + + True + + + 3, 198 + + + 154, 17 + + + 13 + + + Lock Pitch and Roll Values + + + CHK_lockrollpitch + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 0 + + + 80, 86 + + + 78, 20 + + + 16 + + + WP_SPEED_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 0 + + + NoControl + + + 6, 89 + + + 54, 13 + + + 17 + + + m/s + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + NAV_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + NAV_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + NAV_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 7 + + + 534, 107 + + + 170, 108 + + + 0 + + + Nav WP + + + groupBox4 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 1 + + + 80, 86 + + + 78, 20 + + + 18 + + + XTRK_ANGLE_CD1 + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 0 + + + NoControl + + + 6, 89 + + + 82, 13 + + + 19 + + + Error Max + + + label10 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + XTRACK_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label11 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + XTRACK_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label17 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + XTRACK_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 7 + + + 182, 241 + + + 170, 110 + + + 2 + + + Crosstrack Correction + + + groupBox6 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 2 + + + 80, 63 + + + 78, 20 + + + 11 + + + THR_HOLD_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + THR_HOLD_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + THR_HOLD_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 5 + + + 6, 241 + + + 170, 110 + + + 3 + + + Altitude Hold + + + groupBox7 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 3 + + + 80, 61 + + + 78, 20 + + + 11 + + + HLD_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 0 + + + NoControl + + + 6, 64 + + + 65, 13 + + + 12 + + + IMAX + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + HLD_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label30 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + HLD_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label31 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 5 + + + 531, 6 + + + 170, 95 + + + 6 + + + Loiter + + + groupBox19 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 4 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label32 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label34 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label35 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 5 + + + 358, 6 + + + 170, 95 + + + 7 + + + Stabilize Yaw + + + groupBox20 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 5 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label36 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label41 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label42 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 5 + + + 182, 6 + + + 170, 95 + + + 8 + + + Stabilize Pitch + + + groupBox21 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 6 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label43 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label45 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label46 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 5 + + + 6, 6 + + + 170, 95 + + + 9 + + + Stabilize Roll + + + groupBox22 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 7 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label47 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label77 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label82 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 5 + + + 358, 107 + + + 170, 91 + + + 10 + + + Rate Yaw + + + groupBox23 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 8 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label84 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label86 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label87 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 5 + + + 182, 107 + + + 170, 91 + + + 11 + + + Rate Pitch + + + groupBox24 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 9 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 0 + + + NoControl + + + 6, 66 + + + 68, 13 + + + 1 + + + IMAX + + + label88 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label90 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label91 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 5 + + + 6, 107 + + + 170, 91 + + + 12 + + + Rate Roll + + + groupBox25 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 10 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 1 + + + AC2 + TabAC2 @@ -216,6 +4449,1072 @@ 1 + + NoControl + + + 30, 277 + + + 61, 13 + + + 37 + + + Waypoints + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 0 + + + NoControl + + + 139, 276 + + + 177, 17 + + + 38 + + + Load Waypoints on connect? + + + CHK_loadwponconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 1 + + + NoControl + + + 30, 252 + + + 103, 18 + + + 36 + + + Track Length + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 2 + + + 139, 250 + + + 67, 20 + + + 35 + + + 17, 17 + + + On the Flight Data Tab + + + NUM_tracklength + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 3 + + + NoControl + + + 579, 67 + + + 102, 17 + + + 34 + + + Alt Warning + + + CHK_speechaltwarning + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 4 + + + NoControl + + + 30, 228 + + + 61, 13 + + + 0 + + + APM Reset + + + label108 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 5 + + + NoControl + + + 139, 227 + + + 163, 17 + + + 1 + + + Reset APM on USB Connect + + + CHK_resetapmonconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 6 + + + Bottom, Left + + + NoControl + + + 33, 411 + + + 144, 17 + + + 2 + + + Mavlink Message Debug + + + CHK_mavdebug + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 7 + + + NoControl + + + 590, 203 + + + 22, 13 + + + 3 + + + RC + + + label107 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 8 + + + 0 + + + 1 + + + 3 + + + 10 + + + 621, 200 + + + 80, 21 + + + 4 + + + CMB_raterc + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 9 + + + NoControl + + + 425, 203 + + + 69, 13 + + + 5 + + + Mode/Status + + + label104 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 10 + + + NoControl + + + 280, 203 + + + 44, 13 + + + 6 + + + Position + + + label103 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 11 + + + NoControl + + + 136, 203 + + + 43, 13 + + + 7 + + + Attitude + + + label102 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 12 + + + NoControl + + + 27, 203 + + + 84, 13 + + + 8 + + + Telemetry Rates + + + label101 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 13 + + + 0 + + + 1 + + + 3 + + + 10 + + + 499, 200 + + + 80, 21 + + + 9 + + + CMB_ratestatus + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 14 + + + 0 + + + 1 + + + 3 + + + 10 + + + 334, 200 + + + 80, 21 + + + 10 + + + CMB_rateposition + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 15 + + + 0 + + + 1 + + + 3 + + + 10 + + + 188, 200 + + + 80, 21 + + + 11 + + + CMB_rateattitude + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 16 + + + NoControl + + + 283, 168 + + + 402, 13 + + + 12 + + + NOTE: The Configuration Tab will NOT display these units, as those are raw values. + + + + label99 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 17 + + + NoControl + + + 30, 176 + + + 65, 13 + + + 13 + + + Speed Units + + + label98 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 18 + + + NoControl + + + 30, 149 + + + 52, 13 + + + 14 + + + Dist Units + + + label97 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 19 + + + 139, 173 + + + 138, 21 + + + 15 + + + CMB_speedunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 20 + + + 139, 146 + + + 138, 21 + + + 16 + + + CMB_distunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 21 + + + NoControl + + + 30, 122 + + + 45, 13 + + + 17 + + + Joystick + + + label96 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 22 + + + NoControl + + + 30, 71 + + + 44, 13 + + + 18 + + + Speech + + + label95 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 23 + + + NoControl + + + 471, 67 + + + 102, 17 + + + 19 + + + Battery Warning + + + CHK_speechbattery + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 24 + + + NoControl + + + 378, 67 + + + 87, 17 + + + 20 + + + Time Interval + + + CHK_speechcustom + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 25 + + + NoControl + + + 322, 67 + + + 56, 17 + + + 21 + + + Mode + + + CHK_speechmode + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 26 + + + NoControl + + + 245, 67 + + + 71, 17 + + + 22 + + + Waypoint + + + CHK_speechwaypoint + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 27 + + + NoControl + + + 30, 47 + + + 57, 13 + + + 23 + + + OSD Color + + + label94 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 28 + + + 139, 40 + + + 138, 21 + + + 24 + + + CMB_osdcolor + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 29 + + + 139, 90 + + + 138, 21 + + + 25 + + + CMB_language + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 30 + + + NoControl + + + 30, 94 + + + 69, 13 + + + 26 + + + UI Language + + + label93 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 31 + + + NoControl + + + 139, 67 + + + 99, 17 + + + 27 + + + Enable Speech + + + CHK_enablespeech + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 32 + + + NoControl + + + 552, 15 + + + 125, 17 + + + 28 + + + Enable HUD Overlay + + + CHK_hudshow + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 33 + + + NoControl + + + 30, 16 + + + 100, 23 + + + 29 + + + Video Device + + + label92 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 34 + + + 139, 13 + + + 245, 21 + + + 30 + + + CMB_videosources + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 35 + + + NoControl + + + 139, 117 + + + 99, 23 + + + 31 + + + Joystick Setup + + + BUT_Joystick + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 36 + + + NoControl + + + 471, 11 + + + 75, 23 + + + 32 + + + Stop + + + BUT_videostop + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 37 + + + NoControl + + + 390, 11 + + + 75, 23 + + + 33 + + + Start + + + BUT_videostart + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 38 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 2 + + + Planner + TabPlanner @@ -228,6 +5527,18 @@ 2 + + 4, 22 + + + 722, 434 + + + 3 + + + Setup + TabSetup @@ -270,5389 +5581,6 @@ 2 - - groupBox3 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 0 - - - groupBox1 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 1 - - - groupBox2 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 2 - - - groupBox15 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 3 - - - groupBox16 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 4 - - - groupBox14 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 5 - - - groupBox13 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 6 - - - groupBox12 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 7 - - - groupBox11 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 8 - - - groupBox10 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 9 - - - groupBox9 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 10 - - - groupBox8 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 11 - - - 4, 22 - - - 0, 0, 0, 0 - - - 722, 434 - - - 0 - - - APM 2.x - - - THR_FS_VALUE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 0 - - - label5 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 1 - - - THR_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 2 - - - label6 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 3 - - - THR_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 4 - - - label7 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 5 - - - TRIM_THROTTLE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 6 - - - label8 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 7 - - - 405, 217 - - - 195, 108 - - - 0 - - - Throttle 0-100% - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 50, 13 - - - 12 - - - FS Value - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 27, 13 - - - 13 - - - Max - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 24, 13 - - - 14 - - - Min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 15 - - - Cruise - - - ARSPD_RATIO - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 0 - - - label1 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 1 - - - ARSPD_FBW_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 2 - - - label2 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 3 - - - ARSPD_FBW_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 4 - - - label3 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 5 - - - TRIM_ARSPD_CM - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 6 - - - label4 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 7 - - - 406, 325 - - - 195, 108 - - - 1 - - - Airspeed m/s - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 87 - - - 32, 13 - - - 1 - - - Ratio - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 59 - - - 53, 13 - - - 3 - - - FBW max - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 50, 13 - - - 5 - - - FBW min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 64, 13 - - - 6 - - - Cruise * 100 - - - LIM_PITCH_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 0 - - - label39 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 1 - - - LIM_PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 2 - - - label38 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 3 - - - LIM_ROLL_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 4 - - - label37 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 5 - - - 205, 325 - - - 195, 108 - - - 2 - - - Navigation Angles *100 - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 51, 13 - - - 10 - - - Pitch Min - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 54, 13 - - - 11 - - - Pitch Max - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 55, 13 - - - 12 - - - Bank Max - - - XTRK_ANGLE_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 0 - - - label79 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 1 - - - XTRK_GAIN_SC - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 2 - - - label80 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 3 - - - 4, 325 - - - 195, 108 - - - 3 - - - Xtrack Pids - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 8 - - - Entry Angle - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 52, 13 - - - 9 - - - Gain (cm) - - - KFF_PTCH2THR - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 0 - - - label83 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 1 - - - KFF_RDDRMIX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 2 - - - label78 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 3 - - - KFF_PTCHCOMP - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 4 - - - label81 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 5 - - - 205, 217 - - - 195, 108 - - - 4 - - - Other Mix's - - - 111, 13 - - - 78, 20 - - - 13 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 14 - - - P to T - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 61, 13 - - - 15 - - - Rudder Mix - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 16 - - - Pitch Comp - - - ENRGY2THR_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 0 - - - label73 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 1 - - - ENRGY2THR_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 2 - - - label74 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 3 - - - ENRGY2THR_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 4 - - - label75 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 5 - - - ENRGY2THR_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 6 - - - label76 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 7 - - - 4, 217 - - - 195, 108 - - - 5 - - - Energy/Alt Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - ALT2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 0 - - - label69 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 1 - - - ALT2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 2 - - - label70 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 3 - - - ALT2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 4 - - - label71 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 5 - - - ALT2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 6 - - - label72 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 7 - - - 406, 109 - - - 195, 108 - - - 6 - - - Nav Pitch Alt Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - ARSP2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 0 - - - label65 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 1 - - - ARSP2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 2 - - - label66 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 3 - - - ARSP2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 4 - - - label67 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 5 - - - ARSP2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 6 - - - label68 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 7 - - - 205, 109 - - - 195, 108 - - - 7 - - - Nav Pitch AS Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - HDNG2RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 0 - - - label61 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 1 - - - HDNG2RLL_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 2 - - - label62 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 3 - - - HDNG2RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 4 - - - label63 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 5 - - - HDNG2RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 6 - - - label64 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 7 - - - 4, 109 - - - 195, 108 - - - 8 - - - Nav Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - YW2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 0 - - - label57 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 1 - - - YW2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 2 - - - label58 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 3 - - - YW2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 4 - - - label59 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 5 - - - YW2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 6 - - - label60 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 7 - - - 406, 1 - - - 195, 108 - - - 9 - - - Servo Yaw Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - PTCH2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 0 - - - label53 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 1 - - - PTCH2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 2 - - - label54 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 3 - - - PTCH2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 4 - - - label55 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 5 - - - PTCH2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 6 - - - label56 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 7 - - - 205, 1 - - - 195, 108 - - - 10 - - - Servo Pitch Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - RLL2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 0 - - - label49 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 1 - - - RLL2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 2 - - - label50 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 3 - - - RLL2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 4 - - - label51 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 5 - - - RLL2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 6 - - - label52 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 7 - - - 4, 1 - - - 195, 108 - - - 11 - - - Servo Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - CHK_lockrollpitch - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 0 - - - groupBox4 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 1 - - - groupBox6 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 2 - - - groupBox7 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 3 - - - groupBox18 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 4 - - - groupBox19 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 5 - - - groupBox20 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 6 - - - groupBox21 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 7 - - - groupBox22 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 8 - - - groupBox23 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 9 - - - groupBox24 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 10 - - - groupBox25 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 11 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 1 - - - AC2 - - - True - - - 3, 198 - - - 154, 17 - - - 13 - - - Lock Pitch and Roll Values - - - WP_SPEED_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 0 - - - label9 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 1 - - - NAV_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 2 - - - label13 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 3 - - - NAV_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 4 - - - label15 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 5 - - - NAV_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 6 - - - label16 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 7 - - - 534, 107 - - - 170, 108 - - - 0 - - - Nav WP - - - 80, 86 - - - 78, 20 - - - 16 - - - NoControl - - - 6, 89 - - - 54, 13 - - - 17 - - - cm/s - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - XTRK_ANGLE_CD1 - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 0 - - - label10 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 1 - - - XTRACK_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 2 - - - label11 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 3 - - - XTRACK_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 4 - - - label17 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 5 - - - XTRACK_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 6 - - - label18 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 7 - - - 182, 241 - - - 170, 110 - - - 2 - - - Crosstrack Correction - - - 80, 86 - - - 78, 20 - - - 18 - - - NoControl - - - 6, 89 - - - 82, 13 - - - 19 - - - Error Max * 100 - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - THR_HOLD_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 0 - - - label19 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 1 - - - THR_HOLD_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 2 - - - label21 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 3 - - - THR_HOLD_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 4 - - - label22 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 5 - - - 6, 241 - - - 170, 110 - - - 3 - - - Altitude Hold - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 0 - - - label27 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 1 - - - 525, 241 - - - 176, 110 - - - 5 - - - Other Mix's - - - 94, 17 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 20 - - - 82, 13 - - - 10 - - - Pitch Max * 100 - - - HLD_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 0 - - - label28 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 1 - - - HLD_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 2 - - - label30 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 3 - - - HLD_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 4 - - - label31 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 5 - - - 531, 6 - - - 170, 95 - - - 6 - - - Loiter - - - 80, 61 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 64 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 0 - - - label32 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 1 - - - STB_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 2 - - - label34 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 3 - - - STB_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 4 - - - label35 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 5 - - - 358, 6 - - - 170, 95 - - - 7 - - - Stabilize Yaw - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 0 - - - label36 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 1 - - - STB_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 2 - - - label41 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 3 - - - STB_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 4 - - - label42 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 5 - - - 182, 6 - - - 170, 95 - - - 8 - - - Stabilize Pitch - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 0 - - - label43 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 1 - - - STB_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 2 - - - label45 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 3 - - - STB_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 4 - - - label46 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 5 - - - 6, 6 - - - 170, 95 - - - 9 - - - Stabilize Roll - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - RATE_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 0 - - - label47 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 1 - - - RATE_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 2 - - - label77 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 3 - - - RATE_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 4 - - - label82 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 5 - - - 358, 107 - - - 170, 91 - - - 10 - - - Rate Yaw - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 0 - - - label84 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 1 - - - RATE_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 2 - - - label86 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 3 - - - RATE_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 4 - - - label87 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 5 - - - 182, 107 - - - 170, 91 - - - 11 - - - Rate Pitch - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 0 - - - label88 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 1 - - - RATE_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 2 - - - label90 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 3 - - - RATE_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 4 - - - label91 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 5 - - - 6, 107 - - - 170, 91 - - - 12 - - - Rate Roll - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 68, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - label24 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 0 - - - CHK_loadwponconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 1 - - - label23 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 2 - - - NUM_tracklength - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 3 - - - CHK_speechaltwarning - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 4 - - - label108 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 5 - - - CHK_resetapmonconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 6 - - - CHK_mavdebug - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 7 - - - label107 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 8 - - - CMB_raterc - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 9 - - - label104 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 10 - - - label103 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 11 - - - label102 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 12 - - - label101 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 13 - - - CMB_ratestatus - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 14 - - - CMB_rateposition - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 15 - - - CMB_rateattitude - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 16 - - - label99 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 17 - - - label98 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 18 - - - label97 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 19 - - - CMB_speedunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 20 - - - CMB_distunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 21 - - - label96 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 22 - - - label95 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 23 - - - CHK_speechbattery - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 24 - - - CHK_speechcustom - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 25 - - - CHK_speechmode - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 26 - - - CHK_speechwaypoint - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 27 - - - label94 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 28 - - - CMB_osdcolor - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 29 - - - CMB_language - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 30 - - - label93 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 31 - - - CHK_enablespeech - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 32 - - - CHK_hudshow - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 33 - - - label92 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 34 - - - CMB_videosources - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 35 - - - BUT_Joystick - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 36 - - - BUT_videostop - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 37 - - - BUT_videostart - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 38 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 2 - - - Planner - - - NoControl - - - 30, 277 - - - 61, 13 - - - 37 - - - Waypoints - - - NoControl - - - 139, 276 - - - 177, 17 - - - 38 - - - Load Waypoints on connect? - - - NoControl - - - 30, 252 - - - 103, 18 - - - 36 - - - Track Length - - - 17, 17 - - - 139, 250 - - - 67, 20 - - - 35 - - - On the Flight Data Tab - - - NoControl - - - 579, 67 - - - 102, 17 - - - 34 - - - Alt Warning - - - NoControl - - - 30, 228 - - - 61, 13 - - - 0 - - - APM Reset - - - NoControl - - - 139, 227 - - - 163, 17 - - - 1 - - - Reset APM on USB Connect - - - Bottom, Left - - - NoControl - - - 33, 411 - - - 144, 17 - - - 2 - - - Mavlink Message Debug - - - NoControl - - - 590, 203 - - - 22, 13 - - - 3 - - - RC - - - 0 - - - 1 - - - 3 - - - 10 - - - 621, 200 - - - 80, 21 - - - 4 - - - NoControl - - - 425, 203 - - - 69, 13 - - - 5 - - - Mode/Status - - - NoControl - - - 280, 203 - - - 44, 13 - - - 6 - - - Position - - - NoControl - - - 136, 203 - - - 43, 13 - - - 7 - - - Attitude - - - NoControl - - - 27, 203 - - - 84, 13 - - - 8 - - - Telemetry Rates - - - 0 - - - 1 - - - 3 - - - 10 - - - 499, 200 - - - 80, 21 - - - 9 - - - 0 - - - 1 - - - 3 - - - 10 - - - 334, 200 - - - 80, 21 - - - 10 - - - 0 - - - 1 - - - 3 - - - 10 - - - 188, 200 - - - 80, 21 - - - 11 - - - NoControl - - - 283, 168 - - - 402, 13 - - - 12 - - - NOTE: The Configuration Tab will NOT display these units, as those are raw values. - - - - NoControl - - - 30, 176 - - - 65, 13 - - - 13 - - - Speed Units - - - NoControl - - - 30, 149 - - - 52, 13 - - - 14 - - - Dist Units - - - 139, 173 - - - 138, 21 - - - 15 - - - 139, 146 - - - 138, 21 - - - 16 - - - NoControl - - - 30, 122 - - - 45, 13 - - - 17 - - - Joystick - - - NoControl - - - 30, 71 - - - 44, 13 - - - 18 - - - Speech - - - NoControl - - - 471, 67 - - - 102, 17 - - - 19 - - - Battery Warning - - - NoControl - - - 378, 67 - - - 87, 17 - - - 20 - - - Time Interval - - - NoControl - - - 322, 67 - - - 56, 17 - - - 21 - - - Mode - - - NoControl - - - 245, 67 - - - 71, 17 - - - 22 - - - Waypoint - - - NoControl - - - 30, 47 - - - 57, 13 - - - 23 - - - OSD Color - - - 139, 40 - - - 138, 21 - - - 24 - - - 139, 90 - - - 138, 21 - - - 25 - - - NoControl - - - 30, 94 - - - 69, 13 - - - 26 - - - UI Language - - - NoControl - - - 139, 67 - - - 99, 17 - - - 27 - - - Enable Speech - - - NoControl - - - 552, 15 - - - 125, 17 - - - 28 - - - Enable HUD Overlay - - - NoControl - - - 30, 16 - - - 100, 23 - - - 29 - - - Video Device - - - 139, 13 - - - 245, 21 - - - 30 - - - NoControl - - - 139, 117 - - - 99, 23 - - - 31 - - - Joystick Setup - - - NoControl - - - 471, 11 - - - 75, 23 - - - 32 - - - Stop - - - NoControl - - - 390, 11 - - - 75, 23 - - - 33 - - - Start - - - 4, 22 - - - 722, 434 - - - 3 - - - Setup - Bottom, Left diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index 5e8c541c78..77bae958ef 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -436,10 +436,13 @@ namespace ArdupilotMega.GCSViews { this.BeginInvoke((MethodInvoker)delegate() { - tracklog.Value = (int)(MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length * 100); - - lbl_logpercent.Text = (MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length).ToString("0.00%"); + try + { + tracklog.Value = (int)(MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length * 100); + lbl_logpercent.Text = (MainV2.comPort.logplaybackfile.BaseStream.Position / (double)MainV2.comPort.logplaybackfile.BaseStream.Length).ToString("0.00%"); + } + catch { } }); } @@ -861,6 +864,8 @@ namespace ArdupilotMega.GCSViews if (MainV2.comPort.logplaybackfile != null) MainV2.comPort.logplaybackfile.BaseStream.Position = (long)(MainV2.comPort.logplaybackfile.BaseStream.Length * (tracklog.Value / 100.0)); + + updateLogPlayPosition(); } bool loaded = false; @@ -1223,7 +1228,7 @@ namespace ArdupilotMega.GCSViews // Get the TypeCode enumeration. Multiple types get mapped to a common typecode. TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType()); - if (!(typeCode == TypeCode.Single || typeCode == TypeCode.Double || typeCode == TypeCode.Int32)) + if (!(typeCode == TypeCode.Single)) continue; CheckBox chk_box = new CheckBox(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index e144fa180a..496882201c 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -1228,7 +1228,7 @@ namespace ArdupilotMega.GCSViews if (CHK_holdalt.Checked) { - port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist * 100); + port.setParam("ALT_HOLD_RTL", int.Parse(TXT_DefaultAlt.Text) / MainV2.cs.multiplierdist); } else { @@ -1360,7 +1360,7 @@ namespace ArdupilotMega.GCSViews } } - TXT_DefaultAlt.Text = ((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist / 100).ToString("0"); + TXT_DefaultAlt.Text = ((float)param["ALT_HOLD_RTL"] * MainV2.cs.multiplierdist).ToString("0"); TXT_WPRad.Text = ((float)param["WP_RADIUS"] * MainV2.cs.multiplierdist).ToString("0"); try { diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs index b95cd51577..7a993861c8 100644 --- a/Tools/ArdupilotMegaPlanner/HUD.cs +++ b/Tools/ArdupilotMegaPlanner/HUD.cs @@ -23,7 +23,7 @@ using OpenTK.Graphics; namespace hud { - public partial class HUD : GLControl + public class HUD : GLControl// : Graphics { object paintlock = new object(); object streamlock = new object(); @@ -38,7 +38,9 @@ namespace hud int[] charbitmaptexid = new int[6000]; int[] charwidth = new int[6000]; - int huddrawtime = 0; + public int huddrawtime = 0; + + public bool opengl = true; public HUD() { @@ -46,7 +48,7 @@ namespace hud return; InitializeComponent(); - graphicsObject = this; + //graphicsObject = this; //graphicsObject = Graphics.FromImage(objBitmap); } @@ -150,7 +152,6 @@ System.ComponentModel.Category("Values")] Bitmap objBitmap = new Bitmap(1024, 1024); int count = 0; DateTime countdate = DateTime.Now; - HUD graphicsObject; // Graphics DateTime starttime = DateTime.MinValue; @@ -225,14 +226,14 @@ System.ComponentModel.Category("Values")] { //GL.Enable(EnableCap.AlphaTest); - if (this.DesignMode) - { - e.Graphics.Clear(this.BackColor); - e.Graphics.Flush(); - return; + if (this.DesignMode) + { + e.Graphics.Clear(this.BackColor); + e.Graphics.Flush(); + return; } - - if ((DateTime.Now - starttime).TotalMilliseconds < 75 && (_bgimage == null)) + + if ((DateTime.Now - starttime).TotalMilliseconds < 75 && (_bgimage == null)) { //Console.WriteLine("ms "+(DateTime.Now - starttime).TotalMilliseconds); //e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); @@ -241,35 +242,40 @@ System.ComponentModel.Category("Values")] starttime = DateTime.Now; - //Console.WriteLine(DateTime.Now.Millisecond); + if (Console.CursorLeft > 0) + { + //Console.WriteLine(" "+ Console.CursorLeft +" "); + } + + //Console.Write("HUD a "+(DateTime.Now - starttime).TotalMilliseconds); MakeCurrent(); - //GL.LoadIdentity(); - - //GL.ClearColor(Color.Red); + //Console.Write(" b " + (DateTime.Now - starttime).TotalMilliseconds); GL.Clear(ClearBufferMask.ColorBufferBit); - //GL.LoadIdentity(); + //Console.Write(" c " + (DateTime.Now - starttime).TotalMilliseconds); - //GL.Viewport(0, 0, Width, Height); + doPaint(e); - doPaint(); + //Console.Write(" d " + (DateTime.Now - starttime).TotalMilliseconds); this.SwapBuffers(); + //Console.Write(" e " + (DateTime.Now - starttime).TotalMilliseconds); + + count++; + + huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds; + if (DateTime.Now.Second != countdate.Second) { countdate = DateTime.Now; - Console.WriteLine("HUD " + count + " hz drawtime " + huddrawtime); + Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count)); count = 0; + huddrawtime = 0; } - huddrawtime = (int)(DateTime.Now - starttime).TotalMilliseconds; -#if DEBUG - //Console.WriteLine("HUD e " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); -#endif - } void Clear(Color color) @@ -282,7 +288,7 @@ System.ComponentModel.Category("Values")] //graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90); - void DrawArc(Pen penn,RectangleF rect, float start,float degrees) + public void DrawArc(Pen penn,RectangleF rect, float start,float degrees) { //GL.Disable(EnableCap.Texture2D); @@ -309,7 +315,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void DrawEllipse(Pen penn, Rectangle rect) + public void DrawEllipse(Pen penn, Rectangle rect) { //GL.Disable(EnableCap.Texture2D); @@ -340,7 +346,7 @@ System.ComponentModel.Category("Values")] int texture; Bitmap bitmap = new Bitmap(512,512); - void DrawImage(Image img, int x, int y, int width, int height) + public void DrawImage(Image img, int x, int y, int width, int height) { if (img == null) return; @@ -392,7 +398,7 @@ System.ComponentModel.Category("Values")] GL.Disable(EnableCap.Texture2D); } - void DrawPath(Pen penn,GraphicsPath gp) + public void DrawPath(Pen penn, GraphicsPath gp) { try { @@ -401,7 +407,7 @@ System.ComponentModel.Category("Values")] catch { } } - void FillPath(Brush brushh,GraphicsPath gp) + public void FillPath(Brush brushh, GraphicsPath gp) { try { @@ -410,32 +416,32 @@ System.ComponentModel.Category("Values")] catch { } } - void SetClip(Rectangle rect) + public void SetClip(Rectangle rect) { } - void ResetClip() + public void ResetClip() { } - void ResetTransform() + public void ResetTransform() { GL.LoadIdentity(); } - void RotateTransform(float angle) + public void RotateTransform(float angle) { GL.Rotate(angle,0,0,1); } - void TranslateTransform(float x, float y) + public void TranslateTransform(float x, float y) { GL.Translate(x, y, 0f); } - void FillPolygon(Brush brushh, Point[] list) + public void FillPolygon(Brush brushh, Point[] list) { //GL.Disable(EnableCap.Texture2D); @@ -456,7 +462,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void FillPolygon(Brush brushh, PointF[] list) + public void FillPolygon(Brush brushh, PointF[] list) { //GL.Disable(EnableCap.Texture2D); @@ -478,7 +484,7 @@ System.ComponentModel.Category("Values")] //graphicsObject.DrawPolygon(redPen, pointlist); - void DrawPolygon(Pen penn, Point[] list) + public void DrawPolygon(Pen penn, Point[] list) { //GL.Disable(EnableCap.Texture2D); @@ -499,7 +505,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void DrawPolygon(Pen penn, PointF[] list) + public void DrawPolygon(Pen penn, PointF[] list) { //GL.Disable(EnableCap.Texture2D); @@ -523,7 +529,7 @@ System.ComponentModel.Category("Values")] //graphicsObject.FillRectangle(linearBrush, bg); - void FillRectangle(Brush brushh,RectangleF rectf) + public void FillRectangle(Brush brushh, RectangleF rectf) { float x1 = rectf.X; float y1 = rectf.Y; @@ -571,12 +577,12 @@ System.ComponentModel.Category("Values")] //graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height); - void DrawRectangle(Pen penn, RectangleF rect) + public void DrawRectangle(Pen penn, RectangleF rect) { DrawRectangle(penn, rect.X, rect.Y, rect.Width, rect.Height); } - void DrawRectangle(Pen penn,double x1,double y1, double width,double height) + public void DrawRectangle(Pen penn, double x1, double y1, double width, double height) { //GL.Disable(EnableCap.Texture2D); @@ -597,7 +603,7 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void DrawLine(Pen penn,double x1,double y1, double x2,double y2) + public void DrawLine(Pen penn, double x1, double y1, double x2, double y2) { //GL.Disable(EnableCap.Texture2D); @@ -616,8 +622,13 @@ System.ComponentModel.Category("Values")] //GL.Enable(EnableCap.Texture2D); } - void doPaint() + void doPaint(object e) { + HUD graphicsObject = this; + //Graphics graphicsObject = ((PaintEventArgs)e).Graphics; + //graphicsObject.SmoothingMode = SmoothingMode.AntiAlias; + + try { graphicsObject.Clear(Color.Gray); @@ -1131,6 +1142,20 @@ System.ComponentModel.Category("Values")] drawstring(graphicsObject, mode, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + 5); drawstring(graphicsObject, (int)disttowp + ">" + wpno, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + fontsize + 2 + 10); + graphicsObject.DrawLine(greenPen, scrollbg.Left - 5, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20, scrollbg.Left - 5, scrollbg.Top - (int)(fontsize) - 2 - 20); + graphicsObject.DrawLine(greenPen, scrollbg.Left - 10, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 15, scrollbg.Left - 10, scrollbg.Top - (int)(fontsize) - 2 - 20); + graphicsObject.DrawLine(greenPen, scrollbg.Left - 15, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 10, scrollbg.Left - 15, scrollbg.Top - (int)(fontsize ) - 2 - 20); + + drawstring(graphicsObject, ArdupilotMega.MainV2.cs.linkqualitygcs.ToString("0") + "%", font, fontsize, whiteBrush, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20); + if (ArdupilotMega.MainV2.cs.linkqualitygcs == 0) + { + graphicsObject.DrawLine(redPen, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20, scrollbg.Left + 50, scrollbg.Top - (int)(fontsize * 2.2) - 2); + + graphicsObject.DrawLine(redPen, scrollbg.Left, scrollbg.Top - (int)(fontsize * 2.2) - 2, scrollbg.Left + 50, scrollbg.Top - (int)(fontsize * 2.2) - 2 - 20); + } + drawstring(graphicsObject, ArdupilotMega.MainV2.cs.datetime.ToString("HH:mm:ss"), font, fontsize, whiteBrush, scrollbg.Left - 20, scrollbg.Top - fontsize - 2 - 20); + + // battery graphicsObject.ResetTransform(); @@ -1191,8 +1216,6 @@ System.ComponentModel.Category("Values")] Console.WriteLine("hud error "+ex.ToString()); //MessageBox.Show(ex.ToString()); } - - count++; } protected override void OnPaintBackground(PaintEventArgs e) @@ -1355,6 +1378,31 @@ System.ComponentModel.Category("Values")] x += charwidth[charid] * scale; } } + + void drawstring(Graphics e, string text, Font font, float fontsize, Brush brush, float x, float y) + { + if (text == null || text == "") + return; + + pth.Reset(); + + + if (text != null) + pth.AddString(text, font.FontFamily, 0, fontsize + 5, new Point((int)x, (int)y), StringFormat.GenericTypographic); + + //Draw the edge + // this uses lots of cpu time + + //e.SmoothingMode = SmoothingMode.HighSpeed; + + e.DrawPath(P, pth); + + //Draw the face + + e.FillPath(brush, pth); + + //pth.Dispose(); + } protected override void OnResize(EventArgs e) { diff --git a/Tools/ArdupilotMegaPlanner/Log.cs b/Tools/ArdupilotMegaPlanner/Log.cs index e51d159de0..950c676b73 100644 --- a/Tools/ArdupilotMegaPlanner/Log.cs +++ b/Tools/ArdupilotMegaPlanner/Log.cs @@ -375,6 +375,13 @@ namespace ArdupilotMega { Color[] colours = { Color.Red, Color.Orange, Color.Yellow, Color.Green, Color.Blue, Color.Indigo, Color.Violet, Color.Pink }; + AltitudeMode altmode = AltitudeMode.absolute; + + if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) + { + altmode = AltitudeMode.relativeToGround; // because of sonar, this is both right and wrong. right for sonar, wrong in terms of gps as the land slopes off. + } + KMLRoot kml = new KMLRoot(); Folder fldr = new Folder("Log"); @@ -397,7 +404,7 @@ namespace ArdupilotMega continue; LineString ls = new LineString(); - ls.AltitudeMode = AltitudeMode.absolute; + ls.AltitudeMode = altmode; ls.Extrude = true; //ls.Tessellate = true; @@ -484,7 +491,7 @@ namespace ArdupilotMega pmplane.visibility = false; Model model = mod.model; - model.AltitudeMode = AltitudeMode.absolute; + model.AltitudeMode = altmode; model.Scale.x = 2; model.Scale.y = 2; model.Scale.z = 2; @@ -506,7 +513,7 @@ namespace ArdupilotMega catch { } pmplane.Point = new KmlPoint((float)model.Location.longitude, (float)model.Location.latitude, (float)model.Location.altitude); - pmplane.Point.AltitudeMode = AltitudeMode.absolute; + pmplane.Point.AltitudeMode = altmode; Link link = new Link(); link.href = "block_plane_0.dae"; diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 242b2818aa..1f00bcefb7 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -16,6 +16,9 @@ namespace ArdupilotMega { public ICommsSerial BaseStream = new SerialPort(); + /// + /// used for outbound packet sending + /// byte packetcount = 0; public byte sysid = 0; public byte compid = 0; @@ -29,6 +32,8 @@ namespace ArdupilotMega public DateTime lastvalidpacket = DateTime.Now; bool oldlogformat = false; + byte mavlinkversion = 0; + public PointLatLngAlt[] wps = new PointLatLngAlt[200]; public bool debugmavlink = false; @@ -45,7 +50,12 @@ namespace ArdupilotMega public int bps = 0; public DateTime bpstime = DateTime.Now; int recvpacketcount = 0; - int packetslost = 0; + + float synclost; + float packetslost = 0; + float packetsnotlost = 0; + DateTime packetlosttimer = DateTime.Now; + //Stopwatch stopwatch = new Stopwatch(); @@ -169,15 +179,25 @@ namespace ArdupilotMega if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4]) { + __mavlink_heartbeat_t hb = new __mavlink_heartbeat_t(); + + object temp = hb; + + MAVLink.ByteArrayToStructure(buffer, ref temp, 6); + + hb = (MAVLink.__mavlink_heartbeat_t)(temp); + + mavlinkversion = hb.mavlink_version; + sysid = buffer[3]; compid = buffer[4]; recvpacketcount = buffer[2]; - Console.WriteLine("ID " + sysid + " " + compid); + Console.WriteLine("ID sys " + sysid + " comp " + compid + " ver" + mavlinkversion); break; } } - frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + ") "; + frm.Controls[0].Text = "Getting Params.. (sysid " + sysid + " compid "+compid+") "; frm.Refresh(); if (getparams == true) getParamList(); @@ -331,12 +351,28 @@ namespace ArdupilotMega /// struct of data public void generatePacket(byte messageType, object indata) { - byte[] data = StructureToByteArrayEndian(indata); + byte[] data; + + if (mavlinkversion == 3) + { + data = StructureToByteArray(indata); + } + else + { + data = StructureToByteArrayEndian(indata); + } //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); byte[] packet = new byte[data.Length + 6 + 2]; - packet[0] = (byte)'U'; + if (mavlinkversion == 3) + { + packet[0] = 254; + } + else if (mavlinkversion == 2) + { + packet[0] = (byte)'U'; + } packet[1] = (byte)data.Length; packet[2] = packetcount; packet[3] = 255; // this is always 255 - MYGCS @@ -351,6 +387,12 @@ namespace ArdupilotMega } ushort checksum = crc_calculate(packet, packet[1] + 6); + + if (mavlinkversion == 3) + { + checksum = crc_accumulate(MAVLINK_MESSAGE_CRCS[messageType], checksum); + } + byte ck_a = (byte)(checksum & 0xFF); ///< High byte byte ck_b = (byte)(checksum >> 8); ///< Low byte @@ -425,6 +467,8 @@ namespace ArdupilotMega byte[] temp = ASCIIEncoding.ASCII.GetBytes(paramname); + modifyParamForDisplay(false, paramname, ref value); + Array.Resize(ref temp, 15); req.param_id = temp; @@ -458,7 +502,31 @@ namespace ArdupilotMega { if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE) { - param[paramname] = req.param_value; + __mavlink_param_value_t par = new __mavlink_param_value_t(); + + object tempobj = par; + + ByteArrayToStructure(buffer, ref tempobj, 6); + + par = (__mavlink_param_value_t)tempobj; + + string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); + + int pos = st.IndexOf('\0'); + + if (pos != -1) + { + st = st.Substring(0, pos); + } + + if (st != paramname) + { + Console.WriteLine("MAVLINK bad param responce - {0} vs {1}",paramname,st); + continue; + } + + param[st] = (par.param_value); + MainV2.givecomport = false; //System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte return true; @@ -559,6 +627,8 @@ namespace ArdupilotMega st = st.Substring(0, pos); } + modifyParamForDisplay(true, st, ref par.param_value); + param[st] = (par.param_value); a++; @@ -575,17 +645,21 @@ namespace ArdupilotMega return param; } - public _param getParam() + void modifyParamForDisplay(bool fromapm, string paramname, ref float value) { - throw new Exception("getParam Not implemented"); - /* - _param temp = new _param(); - - temp.name = "none"; - temp.value = 0; - - return temp; - */ + if (paramname.ToUpper().EndsWith("_IMAX") || paramname.ToUpper().EndsWith("ALT_HOLD_RTL") || paramname.ToUpper().EndsWith("TRIM_ARSPD_CM") + || paramname.ToUpper().EndsWith("XTRK_ANGLE_CD") || paramname.ToUpper().EndsWith("LIM_PITCH_MAX") || paramname.ToUpper().EndsWith("LIM_PITCH_MIN") + || paramname.ToUpper().EndsWith("LIM_ROLL_CD") || paramname.ToUpper().EndsWith("PITCH_MAX") || paramname.ToUpper().EndsWith("WP_SPEED_MAX")) + { + if (fromapm) + { + value /= 100.0f; + } + else + { + value *= 100.0f; + } + } } /// @@ -1422,7 +1496,7 @@ namespace ArdupilotMega } catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; } - if (temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync + if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync { if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n') { @@ -1437,7 +1511,7 @@ namespace ArdupilotMega // reset count on valid packet readcount = 0; - if (temp[0] == 'U') + if (temp[0] == 'U' || temp[0] == 254) { length = temp[1] + 6 + 2 - 2; // data + header + checksum - U - length if (count >= 5 || logreadmode) @@ -1468,7 +1542,7 @@ namespace ArdupilotMega break; } System.Threading.Thread.Sleep(1); - //System.Windows.Forms.Application.DoEvents(); + System.Windows.Forms.Application.DoEvents(); to++; //Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead); @@ -1493,7 +1567,7 @@ namespace ArdupilotMega if (bpstime.Second != DateTime.Now.Second && !logreadmode) { - Console.WriteLine("bps {0} loss {1} left {2}", bps1, packetslost, BaseStream.BytesToRead); + Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead); bps2 = bps1; // prev sec bps1 = 0; // current sec bpstime = DateTime.Now; @@ -1505,11 +1579,16 @@ namespace ArdupilotMega if (temp.Length >= 5 && temp[3] == 255 && logreadmode) // gcs packet { - return new byte[0]; + return temp;// new byte[0]; } ushort crc = crc_calculate(temp, temp.Length - 2); + if (temp.Length > 5 && temp[0] == 254) + { + crc = crc_accumulate(MAVLINK_MESSAGE_CRCS[temp[5]], crc); + } + if (temp.Length < 5 || temp[temp.Length - 1] != (crc >> 8) || temp[temp.Length - 2] != (crc & 0xff)) { int packetno = 0; @@ -1523,13 +1602,35 @@ namespace ArdupilotMega try { - if (temp[0] == 'U' && temp.Length >= temp[1]) + if ((temp[0] == 'U' || temp[0] == 254) && temp.Length >= temp[1]) { if (temp[2] != ((recvpacketcount + 1) % 0x100)) { - Console.WriteLine("lost {0}", temp[2]); - packetslost++; // actualy sync loss's + synclost++; // actualy sync loss's + + if (temp[2] < ((recvpacketcount + 1) % 0x100)) + { + packetslost += 0x100 - recvpacketcount + temp[2]; + } + else + { + packetslost += temp[2] - recvpacketcount; + } + + Console.WriteLine("lost {0} pkts {1}", temp[2], (int)packetslost); } + + if (packetlosttimer.AddSeconds(10) < DateTime.Now) + { + packetlosttimer = DateTime.Now; + packetslost = (int)(packetslost *0.8f); + packetsnotlost = (int)(packetsnotlost *0.8f); + } + + MainV2.cs.linkqualitygcs = (ushort)((packetsnotlost / (packetsnotlost + packetslost)) * 100); + + packetsnotlost++; + recvpacketcount = temp[2]; //MAVLINK_MSG_ID_GPS_STATUS @@ -1679,6 +1780,8 @@ namespace ArdupilotMega lastlogread = date1; + MainV2.cs.datetime = lastlogread; + int length = 5; int a = 0; while (a < length) @@ -1758,6 +1861,34 @@ namespace ArdupilotMega } public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset) + { + if (bytearray[0] == 'U') + { + ByteArrayToStructureEndian(bytearray, ref obj, startoffset); + } + else + { + int len = Marshal.SizeOf(obj); + + IntPtr i = Marshal.AllocHGlobal(len); + + // create structure from ptr + obj = Marshal.PtrToStructure(i, obj.GetType()); + + try + { + // copy byte array to ptr + Marshal.Copy(bytearray, startoffset, i, len); + } + catch (Exception ex) { Console.WriteLine("ByteArrayToStructure FAIL: error " + ex.ToString()); } + + obj = Marshal.PtrToStructure(i, obj.GetType()); + + Marshal.FreeHGlobal(i); + } + } + + public static void ByteArrayToStructureEndian(byte[] bytearray, ref object obj, int startoffset) { int len = Marshal.SizeOf(obj); @@ -1786,51 +1917,12 @@ namespace ArdupilotMega TypeCode typeCode = Type.GetTypeCode(fieldValue.GetType()); if (typeCode != TypeCode.Object) - {/* - switch (Marshal.SizeOf(fieldValue)) - { - case 1: - Marshal.WriteByte(i, reversestartoffset - 6, bytearray[reversestartoffset]); - break; - case 2: - byte[] temp = new byte[2]; - temp[0] = bytearray[reversestartoffset + 1]; - temp[1] = bytearray[reversestartoffset + 0]; - Marshal.WriteInt16(i, reversestartoffset - 6, BitConverter.ToInt16(temp, 0)); - break; - case 4: - byte[] temp2 = new byte[4]; - temp2[0] = bytearray[reversestartoffset + 3]; - temp2[1] = bytearray[reversestartoffset + 2]; - temp2[2] = bytearray[reversestartoffset + 1]; - temp2[3] = bytearray[reversestartoffset + 0]; - Marshal.WriteInt32(i, reversestartoffset - 6, BitConverter.ToInt32(temp2, 0)); - break; - case 8: - byte[] temp3 = new byte[8]; - temp3[0] = bytearray[reversestartoffset + 7]; - temp3[1] = bytearray[reversestartoffset + 6]; - temp3[2] = bytearray[reversestartoffset + 5]; - temp3[3] = bytearray[reversestartoffset + 4]; - temp3[4] = bytearray[reversestartoffset + 3]; - temp3[5] = bytearray[reversestartoffset + 2]; - temp3[6] = bytearray[reversestartoffset + 1]; - temp3[7] = bytearray[reversestartoffset + 0]; - Marshal.WriteInt64(i, reversestartoffset - 6, BitConverter.ToInt64(temp3, 0)); - break; - default: - Console.WriteLine("bytearraytostruct Bad value"); - break; - } */ + { Array.Reverse(temparray, reversestartoffset, Marshal.SizeOf(fieldValue)); reversestartoffset += Marshal.SizeOf(fieldValue); } else { - /* - for (int c = 0; c < ((byte[])fieldValue).Length;c++) - Marshal.WriteByte(i, c, bytearray[reversestartoffset + c]); - */ reversestartoffset += ((byte[])fieldValue).Length; } diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs index 66c29af6a3..53ac881f15 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs @@ -7,864 +7,1346 @@ namespace ArdupilotMega { partial class MAVLink { - public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS = 150; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_sensor_offsets_t - { - public short mag_ofs_x; ///< magnetometer X offset - public short mag_ofs_y; ///< magnetometer Y offset - public short mag_ofs_z; ///< magnetometer Z offset - public float mag_declination; ///< magnetic declination (radians) - public int raw_press; ///< raw pressure from barometer - public int raw_temp; ///< raw temperature from barometer - public float gyro_cal_x; ///< gyro X calibration - public float gyro_cal_y; ///< gyro Y calibration - public float gyro_cal_z; ///< gyro Z calibration - public float accel_cal_x; ///< accel X calibration - public float accel_cal_y; ///< accel Y calibration - public float accel_cal_z; ///< accel Z calibration - }; - - public const byte MAVLINK_MSG_ID_SET_MAG_OFFSETS = 151; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_mag_offsets_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public short mag_ofs_x; ///< magnetometer X offset - public short mag_ofs_y; ///< magnetometer Y offset - public short mag_ofs_z; ///< magnetometer Z offset - }; - + public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {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}; + public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {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}; public const byte MAVLINK_MSG_ID_MEMINFO = 152; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_meminfo_t { - public ushort brkval; ///< heap top - public ushort freemem; ///< free memory + public ushort brkval; ///< heap top + public ushort freemem; ///< free memory }; - public enum MAV_CMD - { - WAYPOINT = 16, - LOITER_UNLIM = 17, - LOITER_TURNS = 18, - LOITER_TIME = 19, - RETURN_TO_LAUNCH = 20, - LAND = 21, - TAKEOFF = 22, - ROI = 80, - PATHPLANNING = 81, - LAST = 95, - CONDITION_DELAY = 112, - CONDITION_CHANGE_ALT = 113, - CONDITION_DISTANCE = 114, - CONDITION_YAW = 115, - CONDITION_LAST = 159, - DO_SET_MODE = 176, - DO_JUMP = 177, - DO_CHANGE_SPEED = 178, - DO_SET_HOME = 179, - DO_SET_PARAMETER = 180, - DO_SET_RELAY = 181, - DO_REPEAT_RELAY = 182, - DO_SET_SERVO = 183, - DO_REPEAT_SERVO = 184, - DO_CONTROL_VIDEO = 200, - DO_SET_ROI = 201, - DO_LAST = 240, - PREFLIGHT_CALIBRATION = 241, - PREFLIGHT_STORAGE = 245, - }; - - public enum MAV_DATA_STREAM - { - MAV_DATA_STREAM_ALL = 0, - MAV_DATA_STREAM_RAW_SENSORS = 1, - MAV_DATA_STREAM_EXTENDED_STATUS = 2, - MAV_DATA_STREAM_RC_CHANNELS = 3, - MAV_DATA_STREAM_RAW_CONTROLLER = 4, - MAV_DATA_STREAM_POSITION = 6, - MAV_DATA_STREAM_EXTRA1 = 10, - MAV_DATA_STREAM_EXTRA2 = 11, - MAV_DATA_STREAM_EXTRA3 = 12, - }; - - public enum MAV_ROI - { - MAV_ROI_NONE = 0, - MAV_ROI_WPNEXT = 1, - MAV_ROI_WPINDEX = 2, - MAV_ROI_LOCATION = 3, - MAV_ROI_TARGET = 4, - }; - - public const byte MAVLINK_MSG_ID_HEARTBEAT = 0; + public const byte MAVLINK_MSG_ID_MEMINFO_LEN = 4; + public const byte MAVLINK_MSG_ID_152_LEN = 4; + public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS = 150; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_heartbeat_t + public struct __mavlink_sensor_offsets_t { - public byte type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - public byte autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - public byte mavlink_version; ///< MAVLink version + public short mag_ofs_x; ///< magnetometer X offset + public short mag_ofs_y; ///< magnetometer Y offset + public short mag_ofs_z; ///< magnetometer Z offset + public float mag_declination; ///< magnetic declination (radians) + public int raw_press; ///< raw pressure from barometer + public int raw_temp; ///< raw temperature from barometer + public float gyro_cal_x; ///< gyro X calibration + public float gyro_cal_y; ///< gyro Y calibration + public float gyro_cal_z; ///< gyro Z calibration + public float accel_cal_x; ///< accel X calibration + public float accel_cal_y; ///< accel Y calibration + public float accel_cal_z; ///< accel Z calibration }; - public const byte MAVLINK_MSG_ID_BOOT = 1; + public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN = 42; + public const byte MAVLINK_MSG_ID_150_LEN = 42; + public const byte MAVLINK_MSG_ID_SET_MAG_OFFSETS = 151; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_boot_t + public struct __mavlink_set_mag_offsets_t { - public uint version; ///< The onboard software version + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public short mag_ofs_x; ///< magnetometer X offset + public short mag_ofs_y; ///< magnetometer Y offset + public short mag_ofs_z; ///< magnetometer Z offset }; - public const byte MAVLINK_MSG_ID_SYSTEM_TIME = 2; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_system_time_t + public const byte MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN = 8; + public const byte MAVLINK_MSG_ID_151_LEN = 8; + public enum MAV_CMD { - public ulong time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. + WAYPOINT=16, /* Navigate to waypoint. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at waypoint (rotary wing)| Latitude| Longitude| Altitude| */ + LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Turns| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + LOITER_TIME=19, /* Loiter around this waypoint for X seconds |Seconds (decimal)| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + ROI=80, /* Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + DO_CONTROL_VIDEO=200, /* Control onboard camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + devices such as cameras. + |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Empty| Empty| Empty| */ + PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + ENUM_END=246, /* | */ }; - public const byte MAVLINK_MSG_ID_PING = 3; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_ping_t + public enum MAV_DATA_STREAM { - public uint seq; ///< PING sequence - public byte target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - public byte target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - public ulong time; ///< Unix timestamp in microseconds + MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ + MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ + MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ + MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ + MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ + MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_ENUM_END=13, /* | */ }; - public const byte MAVLINK_MSG_ID_SYSTEM_TIME_UTC = 4; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_system_time_utc_t + public enum MAV_ROI { - public uint utc_date; ///< GPS UTC date ddmmyy - public uint utc_time; ///< GPS UTC time hhmmss - }; - - public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL = 5; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_change_operator_control_t - { - public byte target_system; ///< System the GCS requests control for - public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - public byte version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - [MarshalAs(UnmanagedType.ByValArray, SizeConst=25)] - char passkey; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - }; - - public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK = 6; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_change_operator_control_ack_t - { - public byte gcs_system_id; ///< ID of the GCS this message - public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - public byte ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - }; - - public const byte MAVLINK_MSG_ID_AUTH_KEY = 7; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_auth_key_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=32)] - char key; ///< key - }; - - public const byte MAVLINK_MSG_ID_ACTION_ACK = 9; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_action_ack_t - { - public byte action; ///< The action id - public byte result; ///< 0: Action DENIED, 1: Action executed + MAV_ROI_NONE=0, /* No region of interest. | */ + MAV_ROI_WPNEXT=1, /* Point toward next waypoint. | */ + MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ + MAV_ROI_TARGET=4, /* Point toward of given id. | */ + MAV_ROI_ENUM_END=5, /* | */ }; public const byte MAVLINK_MSG_ID_ACTION = 10; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_action_t { - public byte target; ///< The system executing the action - public byte target_component; ///< The component executing the action - public byte action; ///< The action id + public byte target; ///< The system executing the action + public byte target_component; ///< The component executing the action + public byte action; ///< The action id }; - public const byte MAVLINK_MSG_ID_SET_MODE = 11; + public const byte MAVLINK_MSG_ID_ACTION_LEN = 3; + public const byte MAVLINK_MSG_ID_10_LEN = 3; + public const byte MAVLINK_MSG_ID_ACTION_ACK = 9; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_mode_t + public struct __mavlink_action_ack_t { - public byte target; ///< The system setting the mode - public byte mode; ///< The new mode - }; - - public const byte MAVLINK_MSG_ID_SET_NAV_MODE = 12; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_nav_mode_t - { - public byte target; ///< The system setting the mode - public byte nav_mode; ///< The new navigation mode - }; - - public const byte MAVLINK_MSG_ID_PARAM_REQUEST_READ = 20; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_request_read_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - [MarshalAs(UnmanagedType.ByValArray, SizeConst=15)] - public byte[] param_id; ///< Onboard parameter id - public short param_index; ///< Parameter index. Send -1 to use the param ID field as identifier - }; - - public const byte MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_request_list_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - }; - - public const byte MAVLINK_MSG_ID_PARAM_VALUE = 22; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_value_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=15)] - public byte[] param_id; ///< Onboard parameter id - public float param_value; ///< Onboard parameter value - public ushort param_count; ///< Total number of onboard parameters - public ushort param_index; ///< Index of this onboard parameter - }; - - public const byte MAVLINK_MSG_ID_PARAM_SET = 23; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_param_set_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - [MarshalAs(UnmanagedType.ByValArray, SizeConst=15)] - public byte[] param_id; ///< Onboard parameter id - public float param_value; ///< Onboard parameter value - }; - - public const byte MAVLINK_MSG_ID_GPS_RAW_INT = 25; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_raw_int_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - public int lat; ///< Latitude in 1E7 degrees - public int lon; ///< Longitude in 1E7 degrees - public int alt; ///< Altitude in 1E3 meters (millimeters) - public float eph; ///< GPS HDOP - public float epv; ///< GPS VDOP - public float v; ///< GPS ground speed (m/s) - public float hdg; ///< Compass heading in degrees, 0..360 degrees - }; - - public const byte MAVLINK_MSG_ID_SCALED_IMU = 26; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_scaled_imu_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public short xacc; ///< X acceleration (mg) - public short yacc; ///< Y acceleration (mg) - public short zacc; ///< Z acceleration (mg) - public short xgyro; ///< Angular speed around X axis (millirad /sec) - public short ygyro; ///< Angular speed around Y axis (millirad /sec) - public short zgyro; ///< Angular speed around Z axis (millirad /sec) - public short xmag; ///< X Magnetic field (milli tesla) - public short ymag; ///< Y Magnetic field (milli tesla) - public short zmag; ///< Z Magnetic field (milli tesla) - }; - - public const byte MAVLINK_MSG_ID_GPS_STATUS = 27; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_status_t - { - public byte satellites_visible; ///< Number of satellites visible - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_prn; ///< Global satellite ID - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_used; ///< 0: Satellite not used, 1: used for localization - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_elevation; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_azimuth; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. - [MarshalAs(UnmanagedType.ByValArray, SizeConst=20)] - public byte[] satellite_snr; ///< Signal to noise ratio of satellite - }; - - public const byte MAVLINK_MSG_ID_RAW_IMU = 28; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_raw_imu_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public short xacc; ///< X acceleration (raw) - public short yacc; ///< Y acceleration (raw) - public short zacc; ///< Z acceleration (raw) - public short xgyro; ///< Angular speed around X axis (raw) - public short ygyro; ///< Angular speed around Y axis (raw) - public short zgyro; ///< Angular speed around Z axis (raw) - public short xmag; ///< X Magnetic field (raw) - public short ymag; ///< Y Magnetic field (raw) - public short zmag; ///< Z Magnetic field (raw) - }; - - public const byte MAVLINK_MSG_ID_RAW_PRESSURE = 29; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_raw_pressure_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public short press_abs; ///< Absolute pressure (raw) - public short press_diff1; ///< Differential pressure 1 (raw) - public short press_diff2; ///< Differential pressure 2 (raw) - public short temperature; ///< Raw Temperature measurement (raw) - }; - - public const byte MAVLINK_MSG_ID_SCALED_PRESSURE = 38; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_scaled_pressure_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float press_abs; ///< Absolute pressure (hectopascal) - public float press_diff; ///< Differential pressure 1 (hectopascal) - public short temperature; ///< Temperature measurement (0.01 degrees celsius) + public byte action; ///< The action id + public byte result; ///< 0: Action DENIED, 1: Action executed }; + public const byte MAVLINK_MSG_ID_ACTION_ACK_LEN = 2; + public const byte MAVLINK_MSG_ID_9_LEN = 2; public const byte MAVLINK_MSG_ID_ATTITUDE = 30; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_attitude_t { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float roll; ///< Roll angle (rad) - public float pitch; ///< Pitch angle (rad) - public float yaw; ///< Yaw angle (rad) - public float rollspeed; ///< Roll angular speed (rad/s) - public float pitchspeed; ///< Pitch angular speed (rad/s) - public float yawspeed; ///< Yaw angular speed (rad/s) + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) }; - public const byte MAVLINK_MSG_ID_LOCAL_POSITION = 31; + public const byte MAVLINK_MSG_ID_ATTITUDE_LEN = 32; + public const byte MAVLINK_MSG_ID_30_LEN = 32; + public const byte MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT = 60; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_local_position_t + public struct __mavlink_attitude_controller_output_t + { + public byte enabled; ///< 1: enabled, 0: disabled + public byte roll; ///< Attitude roll: -128: -100%, 127: +100% + public byte pitch; ///< Attitude pitch: -128: -100%, 127: +100% + public byte yaw; ///< Attitude yaw: -128: -100%, 127: +100% + public byte thrust; ///< Attitude thrust: -128: -100%, 127: +100% + + }; + + public const byte MAVLINK_MSG_ID_ATTITUDE_NEW = 30; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_attitude_new_t { public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float x; ///< X Position - public float y; ///< Y Position - public float z; ///< Z Position - public float vx; ///< X Speed - public float vy; ///< Y Speed - public float vz; ///< Z Speed + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) + + }; + + public const byte MAVLINK_MSG_ID_AUTH_KEY = 7; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_auth_key_t + { + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=32)] + char key; ///< key + }; + + public const byte MAVLINK_MSG_ID_AUTH_KEY_LEN = 32; + public const byte MAVLINK_MSG_ID_7_LEN = 32; + public const byte MAVLINK_MSG_ID_BOOT = 1; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_boot_t + { + public uint version; ///< The onboard software version + }; + + public const byte MAVLINK_MSG_ID_BOOT_LEN = 4; + public const byte MAVLINK_MSG_ID_1_LEN = 4; + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL = 5; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_change_operator_control_t + { + public byte target_system; ///< System the GCS requests control for + public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + public byte version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=25)] + char passkey; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + }; + + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN = 28; + public const byte MAVLINK_MSG_ID_5_LEN = 28; + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK = 6; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_change_operator_control_ack_t + { + public byte gcs_system_id; ///< ID of the GCS this message + public byte control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + public byte ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + }; + + public const byte MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN = 3; + public const byte MAVLINK_MSG_ID_6_LEN = 3; + public const byte MAVLINK_MSG_ID_COMMAND = 75; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_command_t + { + public byte target_system; ///< System which should execute the command + public byte target_component; ///< Component which should execute the command, 0 for all components + public byte command; ///< Command ID, as defined by MAV_CMD enum. + public byte confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + public float param1; ///< Parameter 1, as defined by MAV_CMD enum. + public float param2; ///< Parameter 2, as defined by MAV_CMD enum. + public float param3; ///< Parameter 3, as defined by MAV_CMD enum. + public float param4; ///< Parameter 4, as defined by MAV_CMD enum. + }; + + public const byte MAVLINK_MSG_ID_COMMAND_LEN = 20; + public const byte MAVLINK_MSG_ID_75_LEN = 20; + public const byte MAVLINK_MSG_ID_COMMAND_ACK = 76; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_command_ack_t + { + public float command; ///< Current airspeed in m/s + public float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + }; + + public const byte MAVLINK_MSG_ID_COMMAND_ACK_LEN = 8; + public const byte MAVLINK_MSG_ID_76_LEN = 8; + public const byte MAVLINK_MSG_ID_CONTROL_STATUS = 52; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_control_status_t + { + public byte position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + public byte vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + public byte gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + public byte ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent + public byte control_att; ///< 0: Attitude control disabled, 1: enabled + public byte control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled + public byte control_pos_z; ///< 0: Z position control disabled, 1: enabled + public byte control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled + }; + + public const byte MAVLINK_MSG_ID_CONTROL_STATUS_LEN = 8; + public const byte MAVLINK_MSG_ID_52_LEN = 8; + public const byte MAVLINK_MSG_ID_DEBUG = 255; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_debug_t + { + public byte ind; ///< index of debug variable + public float value; ///< DEBUG value + }; + + public const byte MAVLINK_MSG_ID_DEBUG_LEN = 5; + public const byte MAVLINK_MSG_ID_255_LEN = 5; + public const byte MAVLINK_MSG_ID_DEBUG_VECT = 251; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_debug_vect_t + { + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=10)] + char name; ///< Name + public ulong usec; ///< Timestamp + public float x; ///< x + public float y; ///< y + public float z; ///< z + }; + + public const byte MAVLINK_MSG_ID_DEBUG_VECT_LEN = 30; + public const byte MAVLINK_MSG_ID_251_LEN = 30; + public const byte MAVLINK_MSG_ID_FULL_STATE = 67; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_full_state_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) + public int lat; ///< Latitude, expressed as * 1E7 + public int lon; ///< Longitude, expressed as * 1E7 + public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + public short xacc; ///< X acceleration (mg) + public short yacc; ///< Y acceleration (mg) + public short zacc; ///< Z acceleration (mg) + }; public const byte MAVLINK_MSG_ID_GLOBAL_POSITION = 33; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_global_position_t { - public ulong usec; ///< Timestamp (microseconds since unix epoch) - public float lat; ///< Latitude, in degrees - public float lon; ///< Longitude, in degrees - public float alt; ///< Absolute altitude, in meters - public float vx; ///< X Speed (in Latitude direction, positive: going north) - public float vy; ///< Y Speed (in Longitude direction, positive: going east) - public float vz; ///< Z Speed (in Altitude direction, positive: going up) + public ulong usec; ///< Timestamp (microseconds since unix epoch) + public float lat; ///< Latitude, in degrees + public float lon; ///< Longitude, in degrees + public float alt; ///< Absolute altitude, in meters + public float vx; ///< X Speed (in Latitude direction, positive: going north) + public float vy; ///< Y Speed (in Longitude direction, positive: going east) + public float vz; ///< Z Speed (in Altitude direction, positive: going up) }; - public const byte MAVLINK_MSG_ID_GPS_RAW = 32; + public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_LEN = 32; + public const byte MAVLINK_MSG_ID_33_LEN = 32; + public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 73; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_raw_t + public struct __mavlink_global_position_int_t { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - public float lat; ///< Latitude in degrees - public float lon; ///< Longitude in degrees - public float alt; ///< Altitude in meters - public float eph; ///< GPS HDOP - public float epv; ///< GPS VDOP - public float v; ///< GPS ground speed - public float hdg; ///< Compass heading in degrees, 0..360 degrees - }; - - public const byte MAVLINK_MSG_ID_SYS_STATUS = 34; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_sys_status_t - { - public byte mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - public byte nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM - public byte status; ///< System status flag, see MAV_STATUS ENUM - public ushort load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - public ushort vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) - public ushort battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) - public ushort packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) - }; - - public const byte MAVLINK_MSG_ID_RC_CHANNELS_RAW = 35; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_rc_channels_raw_t - { - public ushort chan1_raw; ///< RC channel 1 value, in microseconds - public ushort chan2_raw; ///< RC channel 2 value, in microseconds - public ushort chan3_raw; ///< RC channel 3 value, in microseconds - public ushort chan4_raw; ///< RC channel 4 value, in microseconds - public ushort chan5_raw; ///< RC channel 5 value, in microseconds - public ushort chan6_raw; ///< RC channel 6 value, in microseconds - public ushort chan7_raw; ///< RC channel 7 value, in microseconds - public ushort chan8_raw; ///< RC channel 8 value, in microseconds - public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% - }; - - public const byte MAVLINK_MSG_ID_RC_CHANNELS_SCALED = 36; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_rc_channels_scaled_t - { - public short chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public short chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% - }; - - public const byte MAVLINK_MSG_ID_SERVO_OUTPUT_RAW = 37; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_servo_output_raw_t - { - public ushort servo1_raw; ///< Servo output 1 value, in microseconds - public ushort servo2_raw; ///< Servo output 2 value, in microseconds - public ushort servo3_raw; ///< Servo output 3 value, in microseconds - public ushort servo4_raw; ///< Servo output 4 value, in microseconds - public ushort servo5_raw; ///< Servo output 5 value, in microseconds - public ushort servo6_raw; ///< Servo output 6 value, in microseconds - public ushort servo7_raw; ///< Servo output 7 value, in microseconds - public ushort servo8_raw; ///< Servo output 8 value, in microseconds - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT = 39; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort seq; ///< Sequence - public byte frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - public byte command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - public byte current; ///< false:0, true:1 - public byte autocontinue; ///< autocontinue to next wp - public float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - public float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - public float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - public float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - public float x; ///< PARAM5 / local: x position, global: latitude - public float y; ///< PARAM6 / y position: global: longitude - public float z; ///< PARAM7 / z position: global: altitude - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST = 40; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_request_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT = 41; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_set_current_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_CURRENT = 42; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_current_t - { - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST = 43; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_request_list_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_COUNT = 44; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_count_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public ushort count; ///< Number of Waypoints in the Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL = 45; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_clear_all_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_REACHED = 46; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_reached_t - { - public ushort seq; ///< Sequence - }; - - public const byte MAVLINK_MSG_ID_WAYPOINT_ACK = 47; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_waypoint_ack_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public byte type; ///< 0: OK, 1: Error - }; - - public const byte MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN = 48; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_gps_set_global_origin_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public int latitude; ///< global position * 1E7 - public int longitude; ///< global position * 1E7 - public int altitude; ///< global position * 1000 + public int lat; ///< Latitude, expressed as * 1E7 + public int lon; ///< Longitude, expressed as * 1E7 + public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 }; + public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN = 18; + public const byte MAVLINK_MSG_ID_73_LEN = 18; public const byte MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET = 49; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_gps_local_origin_set_t { - public int latitude; ///< Latitude (WGS84), expressed as * 1E7 - public int longitude; ///< Longitude (WGS84), expressed as * 1E7 - public int altitude; ///< Altitude(WGS84), expressed as * 1000 + public int latitude; ///< Latitude (WGS84), expressed as * 1E7 + public int longitude; ///< Longitude (WGS84), expressed as * 1E7 + public int altitude; ///< Altitude(WGS84), expressed as * 1000 }; - public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET = 50; + public const byte MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN = 12; + public const byte MAVLINK_MSG_ID_49_LEN = 12; + public const byte MAVLINK_MSG_ID_GPS_RAW = 32; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_local_position_setpoint_set_t + public struct __mavlink_gps_raw_t { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public float x; ///< x position - public float y; ///< y position - public float z; ///< z position - public float yaw; ///< Desired yaw angle + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + public float lat; ///< Latitude in degrees + public float lon; ///< Longitude in degrees + public float alt; ///< Altitude in meters + public float eph; ///< GPS HDOP + public float epv; ///< GPS VDOP + public float v; ///< GPS ground speed + public float hdg; ///< Compass heading in degrees, 0..360 degrees }; + public const byte MAVLINK_MSG_ID_GPS_RAW_LEN = 37; + public const byte MAVLINK_MSG_ID_32_LEN = 37; + public const byte MAVLINK_MSG_ID_GPS_RAW_INT = 25; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_gps_raw_int_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public byte fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + public int lat; ///< Latitude in 1E7 degrees + public int lon; ///< Longitude in 1E7 degrees + public int alt; ///< Altitude in 1E3 meters (millimeters) + public float eph; ///< GPS HDOP + public float epv; ///< GPS VDOP + public float v; ///< GPS ground speed (m/s) + public float hdg; ///< Compass heading in degrees, 0..360 degrees + }; + + public const byte MAVLINK_MSG_ID_GPS_RAW_INT_LEN = 37; + public const byte MAVLINK_MSG_ID_25_LEN = 37; + public const byte MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN = 48; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_gps_set_global_origin_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public int latitude; ///< global position * 1E7 + public int longitude; ///< global position * 1E7 + public int altitude; ///< global position * 1000 + }; + + public const byte MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN = 14; + public const byte MAVLINK_MSG_ID_48_LEN = 14; + public const byte MAVLINK_MSG_ID_GPS_STATUS = 27; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_gps_status_t + { + public byte satellites_visible; ///< Number of satellites visible + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_prn; ///< Global satellite ID + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_used; ///< 0: Satellite not used, 1: used for localization + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_elevation; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_azimuth; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + public byte[] satellite_snr; ///< Signal to noise ratio of satellite + }; + + public const byte MAVLINK_MSG_ID_GPS_STATUS_LEN = 101; + public const byte MAVLINK_MSG_ID_27_LEN = 101; + public const byte MAVLINK_MSG_ID_HEARTBEAT = 0; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_heartbeat_t + { + public byte type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + public byte autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + public byte mavlink_version; ///< MAVLink version + }; + + public const byte MAVLINK_MSG_ID_HEARTBEAT_LEN = 3; + public const byte MAVLINK_MSG_ID_0_LEN = 3; + public const byte MAVLINK_MSG_ID_HIL_CONTROLS = 68; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_hil_controls_t + { + public ulong time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll_ailerons; ///< Control output -3 .. 1 + public float pitch_elevator; ///< Control output -1 .. 1 + public float yaw_rudder; ///< Control output -1 .. 1 + public float throttle; ///< Throttle 0 .. 1 + public byte mode; ///< System mode (MAV_MODE) + public byte nav_mode; ///< Navigation mode (MAV_NAV_MODE) + }; + + public const byte MAVLINK_MSG_ID_HIL_CONTROLS_LEN = 26; + public const byte MAVLINK_MSG_ID_68_LEN = 26; + public const byte MAVLINK_MSG_ID_HIL_STATE = 67; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_hil_state_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float roll; ///< Roll angle (rad) + public float pitch; ///< Pitch angle (rad) + public float yaw; ///< Yaw angle (rad) + public float rollspeed; ///< Roll angular speed (rad/s) + public float pitchspeed; ///< Pitch angular speed (rad/s) + public float yawspeed; ///< Yaw angular speed (rad/s) + public int lat; ///< Latitude, expressed as * 1E7 + public int lon; ///< Longitude, expressed as * 1E7 + public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + public short xacc; ///< X acceleration (mg) + public short yacc; ///< Y acceleration (mg) + public short zacc; ///< Z acceleration (mg) + }; + + public const byte MAVLINK_MSG_ID_HIL_STATE_LEN = 56; + public const byte MAVLINK_MSG_ID_67_LEN = 56; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION = 31; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_local_position_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float x; ///< X Position + public float y; ///< Y Position + public float z; ///< Z Position + public float vx; ///< X Speed + public float vy; ///< Y Speed + public float vz; ///< Z Speed + }; + + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_LEN = 32; + public const byte MAVLINK_MSG_ID_31_LEN = 32; public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT = 51; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_local_position_setpoint_t { - public float x; ///< x position - public float y; ///< y position - public float z; ///< z position - public float yaw; ///< Desired yaw angle + public float x; ///< x position + public float y; ///< y position + public float z; ///< z position + public float yaw; ///< Desired yaw angle }; - public const byte MAVLINK_MSG_ID_CONTROL_STATUS = 52; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN = 16; + public const byte MAVLINK_MSG_ID_51_LEN = 16; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET = 50; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_control_status_t + public struct __mavlink_local_position_setpoint_set_t { - public byte position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - public byte vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - public byte gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - public byte ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent - public byte control_att; ///< 0: Attitude control disabled, 1: enabled - public byte control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled - public byte control_pos_z; ///< 0: Z position control disabled, 1: enabled - public byte control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float x; ///< x position + public float y; ///< y position + public float z; ///< z position + public float yaw; ///< Desired yaw angle }; - public const byte MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA = 53; + public const byte MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN = 18; + public const byte MAVLINK_MSG_ID_50_LEN = 18; + public const byte MAVLINK_MSG_ID_MANUAL_CONTROL = 69; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_safety_set_allowed_area_t + public struct __mavlink_manual_control_t { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - public float p1x; ///< x position 1 / Latitude 1 - public float p1y; ///< y position 1 / Longitude 1 - public float p1z; ///< z position 1 / Altitude 1 - public float p2x; ///< x position 2 / Latitude 2 - public float p2y; ///< y position 2 / Longitude 2 - public float p2z; ///< z position 2 / Altitude 2 + public byte target; ///< The system to be controlled + public float roll; ///< roll + public float pitch; ///< pitch + public float yaw; ///< yaw + public float thrust; ///< thrust + public byte roll_manual; ///< roll control enabled auto:0, manual:1 + public byte pitch_manual; ///< pitch auto:0, manual:1 + public byte yaw_manual; ///< yaw auto:0, manual:1 + public byte thrust_manual; ///< thrust auto:0, manual:1 }; - public const byte MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA = 54; + public const byte MAVLINK_MSG_ID_MANUAL_CONTROL_LEN = 21; + public const byte MAVLINK_MSG_ID_69_LEN = 21; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_FLOAT = 252; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_safety_allowed_area_t + public struct __mavlink_named_value_float_t { - public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - public float p1x; ///< x position 1 / Latitude 1 - public float p1y; ///< y position 1 / Longitude 1 - public float p1z; ///< z position 1 / Altitude 1 - public float p2x; ///< x position 2 / Latitude 2 - public float p2y; ///< y position 2 / Longitude 2 - public float p2z; ///< z position 2 / Altitude 2 + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=10)] + char name; ///< Name of the debug variable + public float value; ///< Floating point value }; - public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST = 55; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN = 14; + public const byte MAVLINK_MSG_ID_252_LEN = 14; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_INT = 253; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_roll_pitch_yaw_thrust_t + public struct __mavlink_named_value_int_t { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public float roll; ///< Desired roll angle in radians - public float pitch; ///< Desired pitch angle in radians - public float yaw; ///< Desired yaw angle in radians - public float thrust; ///< Collective thrust, normalized to 0 .. 1 - }; - - public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST = 56; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_roll_pitch_yaw_speed_thrust_t - { - public byte target_system; ///< System ID - public byte target_component; ///< Component ID - public float roll_speed; ///< Desired roll angular speed in rad/s - public float pitch_speed; ///< Desired pitch angular speed in rad/s - public float yaw_speed; ///< Desired yaw angular speed in rad/s - public float thrust; ///< Collective thrust, normalized to 0 .. 1 - }; - - public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT = 57; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_roll_pitch_yaw_thrust_setpoint_t - { - public uint time_ms; ///< Timestamp in milliseconds since system boot - public float roll; ///< Desired roll angle in radians - public float pitch; ///< Desired pitch angle in radians - public float yaw; ///< Desired yaw angle in radians - public float thrust; ///< Collective thrust, normalized to 0 .. 1 - }; - - public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT = 58; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t - { - public uint time_ms; ///< Timestamp in milliseconds since system boot - public float roll_speed; ///< Desired roll angular speed in rad/s - public float pitch_speed; ///< Desired pitch angular speed in rad/s - public float yaw_speed; ///< Desired yaw angular speed in rad/s - public float thrust; ///< Collective thrust, normalized to 0 .. 1 + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=10)] + char name; ///< Name of the debug variable + public int value; ///< Signed integer value }; + public const byte MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN = 14; + public const byte MAVLINK_MSG_ID_253_LEN = 14; public const byte MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT = 62; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_nav_controller_output_t { - public float nav_roll; ///< Current desired roll in degrees - public float nav_pitch; ///< Current desired pitch in degrees - public short nav_bearing; ///< Current desired heading in degrees - public short target_bearing; ///< Bearing to current waypoint/target in degrees - public ushort wp_dist; ///< Distance to active waypoint in meters - public float alt_error; ///< Current altitude error in meters - public float aspd_error; ///< Current airspeed error in meters/second - public float xtrack_error; ///< Current crosstrack error on x-y plane in meters + public float nav_roll; ///< Current desired roll in degrees + public float nav_pitch; ///< Current desired pitch in degrees + public short nav_bearing; ///< Current desired heading in degrees + public short target_bearing; ///< Bearing to current waypoint/target in degrees + public ushort wp_dist; ///< Distance to active waypoint in meters + public float alt_error; ///< Current altitude error in meters + public float aspd_error; ///< Current airspeed error in meters/second + public float xtrack_error; ///< Current crosstrack error on x-y plane in meters + }; + + public const byte MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN = 26; + public const byte MAVLINK_MSG_ID_62_LEN = 26; + public const byte MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT = 140; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_object_detection_event_t + { + public uint time; ///< Timestamp in milliseconds since system boot + public ushort object_id; ///< Object ID + public byte type; ///< Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=20)] + char name; ///< Name of the object as defined by the detector + public byte quality; ///< Detection quality / confidence. 0: bad, 255: maximum confidence + public float bearing; ///< Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + public float distance; ///< Ground distance in meters + }; + + public const byte MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT_LEN = 36; + public const byte MAVLINK_MSG_ID_140_LEN = 36; + public const byte MAVLINK_MSG_ID_OPTICAL_FLOW = 100; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_optical_flow_t + { + public ulong time; ///< Timestamp (UNIX) + public byte sensor_id; ///< Sensor ID + public short flow_x; ///< Flow in pixels in x-sensor direction + public short flow_y; ///< Flow in pixels in y-sensor direction + public byte quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + public float ground_distance; ///< Ground distance in meters + }; + + public const byte MAVLINK_MSG_ID_OPTICAL_FLOW_LEN = 18; + public const byte MAVLINK_MSG_ID_100_LEN = 18; + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_request_list_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + }; + + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN = 2; + public const byte MAVLINK_MSG_ID_21_LEN = 2; + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_READ = 20; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_request_read_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=15)] + public byte[] param_id; ///< Onboard parameter id + public short param_index; ///< Parameter index. Send -1 to use the param ID field as identifier + }; + + public const byte MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN = 19; + public const byte MAVLINK_MSG_ID_20_LEN = 19; + public const byte MAVLINK_MSG_ID_PARAM_SET = 23; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_set_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=15)] + public byte[] param_id; ///< Onboard parameter id + public float param_value; ///< Onboard parameter value + }; + + public const byte MAVLINK_MSG_ID_PARAM_SET_LEN = 21; + public const byte MAVLINK_MSG_ID_23_LEN = 21; + public const byte MAVLINK_MSG_ID_PARAM_VALUE = 22; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_param_value_t + { + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=15)] + public byte[] param_id; ///< Onboard parameter id + public float param_value; ///< Onboard parameter value + public ushort param_count; ///< Total number of onboard parameters + public ushort param_index; ///< Index of this onboard parameter + }; + + public const byte MAVLINK_MSG_ID_PARAM_VALUE_LEN = 23; + public const byte MAVLINK_MSG_ID_22_LEN = 23; + public const byte MAVLINK_MSG_ID_PING = 3; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_ping_t + { + public uint seq; ///< PING sequence + public byte target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + public byte target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + public ulong time; ///< Unix timestamp in microseconds + }; + + public const byte MAVLINK_MSG_ID_PING_LEN = 14; + public const byte MAVLINK_MSG_ID_3_LEN = 14; + public const byte MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT = 61; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_position_controller_output_t + { + public byte enabled; ///< 1: enabled, 0: disabled + public byte x; ///< Position x: -128: -100%, 127: +100% + public byte y; ///< Position y: -128: -100%, 127: +100% + public byte z; ///< Position z: -128: -100%, 127: +100% + public byte yaw; ///< Position yaw: -128: -100%, 127: +100% + }; public const byte MAVLINK_MSG_ID_POSITION_TARGET = 63; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_position_target_t { - public float x; ///< x position - public float y; ///< y position - public float z; ///< z position - public float yaw; ///< yaw orientation in radians, 0 = NORTH + public float x; ///< x position + public float y; ///< y position + public float z; ///< z position + public float yaw; ///< yaw orientation in radians, 0 = NORTH }; - public const byte MAVLINK_MSG_ID_STATE_CORRECTION = 64; + public const byte MAVLINK_MSG_ID_POSITION_TARGET_LEN = 16; + public const byte MAVLINK_MSG_ID_63_LEN = 16; + public const byte MAVLINK_MSG_ID_RAW_IMU = 28; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_state_correction_t + public struct __mavlink_raw_imu_t { - public float xErr; ///< x position error - public float yErr; ///< y position error - public float zErr; ///< z position error - public float rollErr; ///< roll error (radians) - public float pitchErr; ///< pitch error (radians) - public float yawErr; ///< yaw error (radians) - public float vxErr; ///< x velocity - public float vyErr; ///< y velocity - public float vzErr; ///< z velocity + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public short xacc; ///< X acceleration (raw) + public short yacc; ///< Y acceleration (raw) + public short zacc; ///< Z acceleration (raw) + public short xgyro; ///< Angular speed around X axis (raw) + public short ygyro; ///< Angular speed around Y axis (raw) + public short zgyro; ///< Angular speed around Z axis (raw) + public short xmag; ///< X Magnetic field (raw) + public short ymag; ///< Y Magnetic field (raw) + public short zmag; ///< Z Magnetic field (raw) }; - public const byte MAVLINK_MSG_ID_SET_ALTITUDE = 65; + public const byte MAVLINK_MSG_ID_RAW_IMU_LEN = 26; + public const byte MAVLINK_MSG_ID_28_LEN = 26; + public const byte MAVLINK_MSG_ID_RAW_PRESSURE = 29; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_set_altitude_t + public struct __mavlink_raw_pressure_t { - public byte target; ///< The system setting the altitude - public uint mode; ///< The new altitude in meters - }; - - public const byte MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_request_data_stream_t - { - public byte target_system; ///< The target requested to send the message stream. - public byte target_component; ///< The target requested to send the message stream. - public byte req_stream_id; ///< The ID of the requested message type - public ushort req_message_rate; ///< Update rate in Hertz - public byte start_stop; ///< 1 to start sending, 0 to stop sending. - }; - - public const byte MAVLINK_MSG_ID_HIL_STATE = 67; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_hil_state_t - { - public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float roll; ///< Roll angle (rad) - public float pitch; ///< Pitch angle (rad) - public float yaw; ///< Yaw angle (rad) - public float rollspeed; ///< Roll angular speed (rad/s) - public float pitchspeed; ///< Pitch angular speed (rad/s) - public float yawspeed; ///< Yaw angular speed (rad/s) - public int lat; ///< Latitude, expressed as * 1E7 - public int lon; ///< Longitude, expressed as * 1E7 - public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - public short xacc; ///< X acceleration (mg) - public short yacc; ///< Y acceleration (mg) - public short zacc; ///< Z acceleration (mg) - }; - - public const byte MAVLINK_MSG_ID_HIL_CONTROLS = 68; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_hil_controls_t - { - }; - - public const byte MAVLINK_MSG_ID_MANUAL_CONTROL = 69; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_manual_control_t - { - public byte target; ///< The system to be controlled - public float roll; ///< roll - public float pitch; ///< pitch - public float yaw; ///< yaw - public float thrust; ///< thrust - public byte roll_manual; ///< roll control enabled auto:0, manual:1 - public byte pitch_manual; ///< pitch auto:0, manual:1 - public byte yaw_manual; ///< yaw auto:0, manual:1 - public byte thrust_manual; ///< thrust auto:0, manual:1 + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public short press_abs; ///< Absolute pressure (raw) + public short press_diff1; ///< Differential pressure 1 (raw) + public short press_diff2; ///< Differential pressure 2 (raw) + public short temperature; ///< Raw Temperature measurement (raw) }; + public const byte MAVLINK_MSG_ID_RAW_PRESSURE_LEN = 16; + public const byte MAVLINK_MSG_ID_29_LEN = 16; public const byte MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE = 70; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_rc_channels_override_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort chan1_raw; ///< RC channel 1 value, in microseconds + public ushort chan2_raw; ///< RC channel 2 value, in microseconds + public ushort chan3_raw; ///< RC channel 3 value, in microseconds + public ushort chan4_raw; ///< RC channel 4 value, in microseconds + public ushort chan5_raw; ///< RC channel 5 value, in microseconds + public ushort chan6_raw; ///< RC channel 6 value, in microseconds + public ushort chan7_raw; ///< RC channel 7 value, in microseconds + public ushort chan8_raw; ///< RC channel 8 value, in microseconds + }; + + public const byte MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN = 18; + public const byte MAVLINK_MSG_ID_70_LEN = 18; + public const byte MAVLINK_MSG_ID_RC_CHANNELS_RAW = 35; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_rc_channels_raw_t + { + public ushort chan1_raw; ///< RC channel 1 value, in microseconds + public ushort chan2_raw; ///< RC channel 2 value, in microseconds + public ushort chan3_raw; ///< RC channel 3 value, in microseconds + public ushort chan4_raw; ///< RC channel 4 value, in microseconds + public ushort chan5_raw; ///< RC channel 5 value, in microseconds + public ushort chan6_raw; ///< RC channel 6 value, in microseconds + public ushort chan7_raw; ///< RC channel 7 value, in microseconds + public ushort chan8_raw; ///< RC channel 8 value, in microseconds + public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% + }; + + public const byte MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN = 17; + public const byte MAVLINK_MSG_ID_35_LEN = 17; + public const byte MAVLINK_MSG_ID_RC_CHANNELS_SCALED = 36; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_rc_channels_scaled_t + { + public short chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public short chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + public byte rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% + }; + + public const byte MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN = 17; + public const byte MAVLINK_MSG_ID_36_LEN = 17; + public const byte MAVLINK_MSG_ID_REQUEST_DATA_STREAM = 66; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_request_data_stream_t + { + public byte target_system; ///< The target requested to send the message stream. + public byte target_component; ///< The target requested to send the message stream. + public byte req_stream_id; ///< The ID of the requested message type + public ushort req_message_rate; ///< Update rate in Hertz + public byte start_stop; ///< 1 to start sending, 0 to stop sending. + }; + + public const byte MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN = 6; + public const byte MAVLINK_MSG_ID_66_LEN = 6; + public const byte MAVLINK_MSG_ID_REQUEST_DYNAMIC_GYRO_CALIBRATION = 67; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_request_dynamic_gyro_calibration_t + { + public byte target_system; ///< The system which should auto-calibrate + public byte target_component; ///< The system component which should auto-calibrate + public float mode; ///< The current ground-truth rpm + public byte axis; ///< The axis to calibrate: 0 roll, 1 pitch, 2 yaw + public ushort time; ///< The time to average over in ms + + }; + + public const byte MAVLINK_MSG_ID_REQUEST_STATIC_CALIBRATION = 68; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_request_static_calibration_t + { + public byte target_system; ///< The system which should auto-calibrate + public byte target_component; ///< The system component which should auto-calibrate + public ushort time; ///< The time to average over in ms + + }; + + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT = 58; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t + { + public ulong time_us; ///< Timestamp in micro seconds since unix epoch + public float roll_speed; ///< Desired roll angular speed in rad/s + public float pitch_speed; ///< Desired pitch angular speed in rad/s + public float yaw_speed; ///< Desired yaw angular speed in rad/s + public float thrust; ///< Collective thrust, normalized to 0 .. 1 + }; + + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN = 24; + public const byte MAVLINK_MSG_ID_58_LEN = 24; + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT = 57; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_roll_pitch_yaw_thrust_setpoint_t + { + public ulong time_us; ///< Timestamp in micro seconds since unix epoch + public float roll; ///< Desired roll angle in radians + public float pitch; ///< Desired pitch angle in radians + public float yaw; ///< Desired yaw angle in radians + public float thrust; ///< Collective thrust, normalized to 0 .. 1 + }; + + public const byte MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN = 24; + public const byte MAVLINK_MSG_ID_57_LEN = 24; + public const byte MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA = 54; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_safety_allowed_area_t + { + public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + public float p1x; ///< x position 1 / Latitude 1 + public float p1y; ///< y position 1 / Longitude 1 + public float p1z; ///< z position 1 / Altitude 1 + public float p2x; ///< x position 2 / Latitude 2 + public float p2y; ///< y position 2 / Longitude 2 + public float p2z; ///< z position 2 / Altitude 2 + }; + + public const byte MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN = 25; + public const byte MAVLINK_MSG_ID_54_LEN = 25; + public const byte MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA = 53; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_safety_set_allowed_area_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public byte frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + public float p1x; ///< x position 1 / Latitude 1 + public float p1y; ///< y position 1 / Longitude 1 + public float p1z; ///< z position 1 / Altitude 1 + public float p2x; ///< x position 2 / Latitude 2 + public float p2y; ///< y position 2 / Longitude 2 + public float p2z; ///< z position 2 / Altitude 2 + }; + + public const byte MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN = 27; + public const byte MAVLINK_MSG_ID_53_LEN = 27; + public const byte MAVLINK_MSG_ID_SCALED_IMU = 26; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_scaled_imu_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public short xacc; ///< X acceleration (mg) + public short yacc; ///< Y acceleration (mg) + public short zacc; ///< Z acceleration (mg) + public short xgyro; ///< Angular speed around X axis (millirad /sec) + public short ygyro; ///< Angular speed around Y axis (millirad /sec) + public short zgyro; ///< Angular speed around Z axis (millirad /sec) + public short xmag; ///< X Magnetic field (milli tesla) + public short ymag; ///< Y Magnetic field (milli tesla) + public short zmag; ///< Z Magnetic field (milli tesla) + }; + + public const byte MAVLINK_MSG_ID_SCALED_IMU_LEN = 26; + public const byte MAVLINK_MSG_ID_26_LEN = 26; + public const byte MAVLINK_MSG_ID_SCALED_PRESSURE = 38; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_scaled_pressure_t + { + public ulong usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + public float press_abs; ///< Absolute pressure (hectopascal) + public float press_diff; ///< Differential pressure 1 (hectopascal) + public short temperature; ///< Temperature measurement (0.01 degrees celsius) + }; + + public const byte MAVLINK_MSG_ID_SCALED_PRESSURE_LEN = 18; + public const byte MAVLINK_MSG_ID_38_LEN = 18; + public const byte MAVLINK_MSG_ID_SERVO_OUTPUT_RAW = 37; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_servo_output_raw_t + { + public ushort servo1_raw; ///< Servo output 1 value, in microseconds + public ushort servo2_raw; ///< Servo output 2 value, in microseconds + public ushort servo3_raw; ///< Servo output 3 value, in microseconds + public ushort servo4_raw; ///< Servo output 4 value, in microseconds + public ushort servo5_raw; ///< Servo output 5 value, in microseconds + public ushort servo6_raw; ///< Servo output 6 value, in microseconds + public ushort servo7_raw; ///< Servo output 7 value, in microseconds + public ushort servo8_raw; ///< Servo output 8 value, in microseconds + }; + + public const byte MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN = 16; + public const byte MAVLINK_MSG_ID_37_LEN = 16; + public const byte MAVLINK_MSG_ID_SET_ALTITUDE = 65; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_altitude_t + { + public byte target; ///< The system setting the altitude + public uint mode; ///< The new altitude in meters + }; + + public const byte MAVLINK_MSG_ID_SET_ALTITUDE_LEN = 5; + public const byte MAVLINK_MSG_ID_65_LEN = 5; + public const byte MAVLINK_MSG_ID_SET_MODE = 11; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_mode_t + { + public byte target; ///< The system setting the mode + public byte mode; ///< The new mode + }; + + public const byte MAVLINK_MSG_ID_SET_MODE_LEN = 2; + public const byte MAVLINK_MSG_ID_11_LEN = 2; + public const byte MAVLINK_MSG_ID_SET_NAV_MODE = 12; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_nav_mode_t + { + public byte target; ///< The system setting the mode + public byte nav_mode; ///< The new navigation mode + }; + + public const byte MAVLINK_MSG_ID_SET_NAV_MODE_LEN = 2; + public const byte MAVLINK_MSG_ID_12_LEN = 2; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW = 55; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_set_roll_pitch_yaw_t { public byte target_system; ///< System ID public byte target_component; ///< Component ID - public ushort chan1_raw; ///< RC channel 1 value, in microseconds - public ushort chan2_raw; ///< RC channel 2 value, in microseconds - public ushort chan3_raw; ///< RC channel 3 value, in microseconds - public ushort chan4_raw; ///< RC channel 4 value, in microseconds - public ushort chan5_raw; ///< RC channel 5 value, in microseconds - public ushort chan6_raw; ///< RC channel 6 value, in microseconds - public ushort chan7_raw; ///< RC channel 7 value, in microseconds - public ushort chan8_raw; ///< RC channel 8 value, in microseconds + public float roll; ///< Desired roll angle in radians + public float pitch; ///< Desired pitch angle in radians + public float yaw; ///< Desired yaw angle in radians + }; - public const byte MAVLINK_MSG_ID_GLOBAL_POSITION_INT = 73; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED = 56; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_global_position_int_t + public struct __mavlink_set_roll_pitch_yaw_speed_t { - public int lat; ///< Latitude, expressed as * 1E7 - public int lon; ///< Longitude, expressed as * 1E7 - public int alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - public short vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - public short vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - public short vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float roll_speed; ///< Desired roll angular speed in rad/s + public float pitch_speed; ///< Desired pitch angular speed in rad/s + public float yaw_speed; ///< Desired yaw angular speed in rad/s + }; - public const byte MAVLINK_MSG_ID_VFR_HUD = 74; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST = 56; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_vfr_hud_t + public struct __mavlink_set_roll_pitch_yaw_speed_thrust_t { - public float airspeed; ///< Current airspeed in m/s - public float groundspeed; ///< Current ground speed in m/s - public short heading; ///< Current heading in degrees, in compass units (0..360, 0=north) - public ushort throttle; ///< Current throttle setting in integer percent, 0 to 100 - public float alt; ///< Current altitude (MSL), in meters - public float climb; ///< Current climb rate in meters/second + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float roll_speed; ///< Desired roll angular speed in rad/s + public float pitch_speed; ///< Desired pitch angular speed in rad/s + public float yaw_speed; ///< Desired yaw angular speed in rad/s + public float thrust; ///< Collective thrust, normalized to 0 .. 1 }; - public const byte MAVLINK_MSG_ID_COMMAND = 75; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN = 18; + public const byte MAVLINK_MSG_ID_56_LEN = 18; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST = 55; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_command_t + public struct __mavlink_set_roll_pitch_yaw_thrust_t { - public byte target_system; ///< System which should execute the command - public byte target_component; ///< Component which should execute the command, 0 for all components - public byte command; ///< Command ID, as defined by MAV_CMD enum. - public byte confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - public float param1; ///< Parameter 1, as defined by MAV_CMD enum. - public float param2; ///< Parameter 2, as defined by MAV_CMD enum. - public float param3; ///< Parameter 3, as defined by MAV_CMD enum. - public float param4; ///< Parameter 4, as defined by MAV_CMD enum. + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float roll; ///< Desired roll angle in radians + public float pitch; ///< Desired pitch angle in radians + public float yaw; ///< Desired yaw angle in radians + public float thrust; ///< Collective thrust, normalized to 0 .. 1 }; - public const byte MAVLINK_MSG_ID_COMMAND_ACK = 76; + public const byte MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN = 18; + public const byte MAVLINK_MSG_ID_55_LEN = 18; + public const byte MAVLINK_MSG_ID_STATE_CORRECTION = 64; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_command_ack_t + public struct __mavlink_state_correction_t { - public float command; ///< Current airspeed in m/s - public float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - }; - - public const byte MAVLINK_MSG_ID_DEBUG_VECT = 251; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_debug_vect_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=10)] - char name; ///< Name - public ulong usec; ///< Timestamp - public float x; ///< x - public float y; ///< y - public float z; ///< z - }; - - public const byte MAVLINK_MSG_ID_NAMED_VALUE_FLOAT = 252; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_named_value_float_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=10)] - char name; ///< Name of the debug variable - public float value; ///< Floating point value - }; - - public const byte MAVLINK_MSG_ID_NAMED_VALUE_INT = 253; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_named_value_int_t - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst=10)] - char name; ///< Name of the debug variable - public int value; ///< Signed integer value + public float xErr; ///< x position error + public float yErr; ///< y position error + public float zErr; ///< z position error + public float rollErr; ///< roll error (radians) + public float pitchErr; ///< pitch error (radians) + public float yawErr; ///< yaw error (radians) + public float vxErr; ///< x velocity + public float vyErr; ///< y velocity + public float vzErr; ///< z velocity }; + public const byte MAVLINK_MSG_ID_STATE_CORRECTION_LEN = 36; + public const byte MAVLINK_MSG_ID_64_LEN = 36; public const byte MAVLINK_MSG_ID_STATUSTEXT = 254; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_statustext_t { - public byte severity; ///< Severity of status, 0 = info message, 255 = critical fault - [MarshalAs(UnmanagedType.ByValArray, SizeConst=50)] - public byte[] text; ///< Status text message, without null termination character + public byte severity; ///< Severity of status, 0 = info message, 255 = critical fault + [MarshalAs( + UnmanagedType.ByValArray, + SizeConst=50)] + public byte[] text; ///< Status text message, without null termination character }; - public const byte MAVLINK_MSG_ID_DEBUG = 255; + public const byte MAVLINK_MSG_ID_STATUSTEXT_LEN = 51; + public const byte MAVLINK_MSG_ID_254_LEN = 51; + public const byte MAVLINK_MSG_ID_SYSTEM_TIME = 2; + public const byte MAVLINK_MSG_ID_SYSTEM_TIME_UTC = 4; + public const byte MAVLINK_MSG_ID_SYS_STATUS = 34; [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_debug_t + public struct __mavlink_sys_status_t { - public byte ind; ///< index of debug variable - public float value; ///< DEBUG value + public byte mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + public byte nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + public byte status; ///< System status flag, see MAV_STATUS ENUM + public ushort load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + public ushort vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) + public ushort battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) + public ushort packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) }; -Type[] mavstructs = new Type[] {typeof(__mavlink_heartbeat_t) ,typeof(__mavlink_boot_t) ,typeof(__mavlink_system_time_t) ,typeof(__mavlink_ping_t) ,typeof(__mavlink_system_time_utc_t) ,typeof(__mavlink_change_operator_control_t) ,typeof(__mavlink_change_operator_control_ack_t) ,typeof(__mavlink_auth_key_t) ,null ,typeof(__mavlink_action_ack_t) ,typeof(__mavlink_action_t) ,typeof(__mavlink_set_mode_t) ,typeof(__mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof(__mavlink_param_request_read_t) ,typeof(__mavlink_param_request_list_t) ,typeof(__mavlink_param_value_t) ,typeof(__mavlink_param_set_t) ,null ,typeof(__mavlink_gps_raw_int_t) ,typeof(__mavlink_scaled_imu_t) ,typeof(__mavlink_gps_status_t) ,typeof(__mavlink_raw_imu_t) ,typeof(__mavlink_raw_pressure_t) ,typeof(__mavlink_attitude_t) ,typeof(__mavlink_local_position_t) ,typeof(__mavlink_gps_raw_t) ,typeof(__mavlink_global_position_t) ,typeof(__mavlink_sys_status_t) ,typeof(__mavlink_rc_channels_raw_t) ,typeof(__mavlink_rc_channels_scaled_t) ,typeof(__mavlink_servo_output_raw_t) ,typeof(__mavlink_scaled_pressure_t) ,typeof(__mavlink_waypoint_t) ,typeof(__mavlink_waypoint_request_t) ,typeof(__mavlink_waypoint_set_current_t) ,typeof(__mavlink_waypoint_current_t) ,typeof(__mavlink_waypoint_request_list_t) ,typeof(__mavlink_waypoint_count_t) ,typeof(__mavlink_waypoint_clear_all_t) ,typeof(__mavlink_waypoint_reached_t) ,typeof(__mavlink_waypoint_ack_t) ,typeof(__mavlink_gps_set_global_origin_t) ,typeof(__mavlink_gps_local_origin_set_t) ,typeof(__mavlink_local_position_setpoint_set_t) ,typeof(__mavlink_local_position_setpoint_t) ,typeof(__mavlink_control_status_t) ,typeof(__mavlink_safety_set_allowed_area_t) ,typeof(__mavlink_safety_allowed_area_t) ,typeof(__mavlink_set_roll_pitch_yaw_thrust_t) ,typeof(__mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof(__mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof(__mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,null ,null ,typeof(__mavlink_nav_controller_output_t) ,typeof(__mavlink_position_target_t) ,typeof(__mavlink_state_correction_t) ,typeof(__mavlink_set_altitude_t) ,typeof(__mavlink_request_data_stream_t) ,typeof(__mavlink_hil_state_t) ,typeof(__mavlink_hil_controls_t) ,typeof(__mavlink_manual_control_t) ,typeof(__mavlink_rc_channels_override_t) ,null ,null ,typeof(__mavlink_global_position_int_t) ,typeof(__mavlink_vfr_hud_t) ,typeof(__mavlink_command_t) ,typeof(__mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof(__mavlink_sensor_offsets_t) ,typeof(__mavlink_set_mag_offsets_t) ,typeof(__mavlink_meminfo_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof(__mavlink_debug_vect_t) ,typeof(__mavlink_named_value_float_t) ,typeof(__mavlink_named_value_int_t) ,typeof(__mavlink_statustext_t) ,typeof(__mavlink_debug_t) ,null ,}; + public const byte MAVLINK_MSG_ID_SYS_STATUS_LEN = 11; + public const byte MAVLINK_MSG_ID_34_LEN = 11; + public const byte MAVLINK_MSG_ID_VFR_HUD = 74; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_vfr_hud_t + { + public float airspeed; ///< Current airspeed in m/s + public float groundspeed; ///< Current ground speed in m/s + public short heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + public ushort throttle; ///< Current throttle setting in integer percent, 0 to 100 + public float alt; ///< Current altitude (MSL), in meters + public float climb; ///< Current climb rate in meters/second + }; + + public const byte MAVLINK_MSG_ID_VFR_HUD_LEN = 20; + public const byte MAVLINK_MSG_ID_74_LEN = 20; + public const byte MAVLINK_MSG_ID_WAYPOINT = 39; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort seq; ///< Sequence + public byte frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + public byte command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + public byte current; ///< false:0, true:1 + public byte autocontinue; ///< autocontinue to next wp + public float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + public float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + public float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + public float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + public float x; ///< PARAM5 / local: x position, global: latitude + public float y; ///< PARAM6 / y position: global: longitude + public float z; ///< PARAM7 / z position: global: altitude + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_LEN = 36; + public const byte MAVLINK_MSG_ID_39_LEN = 36; + public const byte MAVLINK_MSG_ID_WAYPOINT_ACK = 47; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_ack_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public byte type; ///< 0: OK, 1: Error + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_ACK_LEN = 3; + public const byte MAVLINK_MSG_ID_47_LEN = 3; + public const byte MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL = 45; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_clear_all_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN = 2; + public const byte MAVLINK_MSG_ID_45_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_COUNT = 44; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_count_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort count; ///< Number of Waypoints in the Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN = 4; + public const byte MAVLINK_MSG_ID_44_LEN = 4; + public const byte MAVLINK_MSG_ID_WAYPOINT_CURRENT = 42; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_current_t + { + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN = 2; + public const byte MAVLINK_MSG_ID_42_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_REACHED = 46; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_reached_t + { + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN = 2; + public const byte MAVLINK_MSG_ID_46_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST = 40; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_request_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN = 4; + public const byte MAVLINK_MSG_ID_40_LEN = 4; + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST = 43; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_request_list_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN = 2; + public const byte MAVLINK_MSG_ID_43_LEN = 2; + public const byte MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT = 41; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_set_current_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public ushort seq; ///< Sequence + }; + + public const byte MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN = 4; + public const byte MAVLINK_MSG_ID_41_LEN = 4; + public const byte MAVLINK_MSG_ID_WAYPOINT_SET_GLOBAL_REFERENCE = 48; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_waypoint_set_global_reference_t + { + public byte target_system; ///< System ID + public byte target_component; ///< Component ID + public float global_x; ///< global x position + public float global_y; ///< global y position + public float global_z; ///< global z position + public float global_yaw; ///< global yaw orientation in radians, 0 = NORTH + public float local_x; ///< local x position that matches the global x position + public float local_y; ///< local y position that matches the global y position + public float local_z; ///< local z position that matches the global z position + public float local_yaw; ///< local yaw that matches the global yaw orientation + + }; + + public enum MAV_CLASS + { + MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything + MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch + MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu + MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com + MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org + MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints + MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands + MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set + MAV_CLASS_NONE = 8, ///< No valid autopilot + MAV_CLASS_NB ///< Number of autopilot classes + }; + + public enum MAV_ACTION + { + MAV_ACTION_HOLD = 0, + MAV_ACTION_MOTORS_START = 1, + MAV_ACTION_LAUNCH = 2, + MAV_ACTION_RETURN = 3, + MAV_ACTION_EMCY_LAND = 4, + MAV_ACTION_EMCY_KILL = 5, + MAV_ACTION_CONFIRM_KILL = 6, + MAV_ACTION_CONTINUE = 7, + MAV_ACTION_MOTORS_STOP = 8, + MAV_ACTION_HALT = 9, + MAV_ACTION_SHUTDOWN = 10, + MAV_ACTION_REBOOT = 11, + MAV_ACTION_SET_MANUAL = 12, + MAV_ACTION_SET_AUTO = 13, + MAV_ACTION_STORAGE_READ = 14, + MAV_ACTION_STORAGE_WRITE = 15, + MAV_ACTION_CALIBRATE_RC = 16, + MAV_ACTION_CALIBRATE_GYRO = 17, + MAV_ACTION_CALIBRATE_MAG = 18, + MAV_ACTION_CALIBRATE_ACC = 19, + MAV_ACTION_CALIBRATE_PRESSURE = 20, + MAV_ACTION_REC_START = 21, + MAV_ACTION_REC_PAUSE = 22, + MAV_ACTION_REC_STOP = 23, + MAV_ACTION_TAKEOFF = 24, + MAV_ACTION_NAVIGATE = 25, + MAV_ACTION_LAND = 26, + MAV_ACTION_LOITER = 27, + MAV_ACTION_SET_ORIGIN = 28, + MAV_ACTION_RELAY_ON = 29, + MAV_ACTION_RELAY_OFF = 30, + MAV_ACTION_GET_IMAGE = 31, + MAV_ACTION_VIDEO_START = 32, + MAV_ACTION_VIDEO_STOP = 33, + MAV_ACTION_RESET_MAP = 34, + MAV_ACTION_RESET_PLAN = 35, + MAV_ACTION_DELAY_BEFORE_COMMAND = 36, + MAV_ACTION_ASCEND_AT_RATE = 37, + MAV_ACTION_CHANGE_MODE = 38, + MAV_ACTION_LOITER_MAX_TURNS = 39, + MAV_ACTION_LOITER_MAX_TIME = 40, + MAV_ACTION_START_HILSIM = 41, + MAV_ACTION_STOP_HILSIM = 42, + MAV_ACTION_NB ///< Number of MAV actions + }; + + public enum MAV_MODE + { + MAV_MODE_UNINIT = 0, ///< System is in undefined state + MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe + MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control + MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint + MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation + MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use + MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use + MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use + MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive + MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back + }; + + public enum MAV_STATE + { + MAV_STATE_UNINIT = 0, + MAV_STATE_BOOT, + MAV_STATE_CALIBRATING, + MAV_STATE_STANDBY, + MAV_STATE_ACTIVE, + MAV_STATE_CRITICAL, + MAV_STATE_EMERGENCY, + MAV_STATE_HILSIM, + MAV_STATE_POWEROFF + }; + + public enum MAV_NAV + { + MAV_NAV_GROUNDED = 0, + MAV_NAV_LIFTOFF, + MAV_NAV_HOLD, + MAV_NAV_WAYPOINT, + MAV_NAV_VECTOR, + MAV_NAV_RETURNING, + MAV_NAV_LANDING, + MAV_NAV_LOST, + MAV_NAV_LOITER, + MAV_NAV_FREE_DRIFT + }; + + public enum MAV_TYPE + { + MAV_GENERIC = 0, + MAV_FIXED_WING = 1, + MAV_QUADROTOR = 2, + MAV_COAXIAL = 3, + MAV_HELICOPTER = 4, + MAV_GROUND = 5, + OCU = 6, + MAV_AIRSHIP = 7, + MAV_FREE_BALLOON = 8, + MAV_ROCKET = 9, + UGV_GROUND_ROVER = 10, + UGV_SURFACE_SHIP = 11 + }; + + public enum MAV_AUTOPILOT_TYPE + { + MAV_AUTOPILOT_GENERIC = 0, + MAV_AUTOPILOT_PIXHAWK = 1, + MAV_AUTOPILOT_SLUGS = 2, + MAV_AUTOPILOT_ARDUPILOTMEGA = 3, + MAV_AUTOPILOT_NONE = 4 + }; + + public enum MAV_COMPONENT + { + MAV_COMP_ID_GPS, + MAV_COMP_ID_WAYPOINTPLANNER, + MAV_COMP_ID_BLOBTRACKER, + MAV_COMP_ID_PATHPLANNER, + MAV_COMP_ID_AIRSLAM, + MAV_COMP_ID_MAPPER, + MAV_COMP_ID_CAMERA, + MAV_COMP_ID_IMU = 200, + MAV_COMP_ID_IMU_2 = 201, + MAV_COMP_ID_IMU_3 = 202, + MAV_COMP_ID_UDP_BRIDGE = 240, + MAV_COMP_ID_UART_BRIDGE = 241, + MAV_COMP_ID_SYSTEM_CONTROL = 250 + }; + + public enum MAV_FRAME + { + MAV_FRAME_GLOBAL = 0, + MAV_FRAME_LOCAL = 1, + MAV_FRAME_MISSION = 2, + MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, + MAV_FRAME_LOCAL_ENU = 4 + }; + +Type[] mavstructs = new Type[] {typeof( __mavlink_heartbeat_t) ,typeof( __mavlink_boot_t) ,null ,typeof( __mavlink_ping_t) ,null ,typeof( __mavlink_change_operator_control_t) ,typeof( __mavlink_change_operator_control_ack_t) ,typeof( __mavlink_auth_key_t) ,null ,typeof( __mavlink_action_ack_t) ,typeof( __mavlink_action_t) ,typeof( __mavlink_set_mode_t) ,typeof( __mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_param_request_read_t) ,typeof( __mavlink_param_request_list_t) ,typeof( __mavlink_param_value_t) ,typeof( __mavlink_param_set_t) ,null ,typeof( __mavlink_gps_raw_int_t) ,typeof( __mavlink_scaled_imu_t) ,typeof( __mavlink_gps_status_t) ,typeof( __mavlink_raw_imu_t) ,typeof( __mavlink_raw_pressure_t) ,typeof( __mavlink_attitude_new_t ) ,typeof( __mavlink_local_position_t) ,typeof( __mavlink_gps_raw_t) ,typeof( __mavlink_global_position_t) ,typeof( __mavlink_sys_status_t) ,typeof( __mavlink_rc_channels_raw_t) ,typeof( __mavlink_rc_channels_scaled_t) ,typeof( __mavlink_servo_output_raw_t) ,typeof( __mavlink_scaled_pressure_t) ,typeof( __mavlink_waypoint_t) ,typeof( __mavlink_waypoint_request_t) ,typeof( __mavlink_waypoint_set_current_t) ,typeof( __mavlink_waypoint_current_t) ,typeof( __mavlink_waypoint_request_list_t) ,typeof( __mavlink_waypoint_count_t) ,typeof( __mavlink_waypoint_clear_all_t) ,typeof( __mavlink_waypoint_reached_t) ,typeof( __mavlink_waypoint_ack_t) ,typeof( __mavlink_waypoint_set_global_reference_t ) ,typeof( __mavlink_gps_local_origin_set_t) ,typeof( __mavlink_local_position_setpoint_set_t) ,typeof( __mavlink_local_position_setpoint_t) ,typeof( __mavlink_control_status_t) ,typeof( __mavlink_safety_set_allowed_area_t) ,typeof( __mavlink_safety_allowed_area_t) ,typeof( __mavlink_set_roll_pitch_yaw_thrust_t) ,typeof( __mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof( __mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof( __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,typeof( __mavlink_attitude_controller_output_t ) ,typeof( __mavlink_position_controller_output_t ) ,typeof( __mavlink_nav_controller_output_t) ,typeof( __mavlink_position_target_t) ,typeof( __mavlink_state_correction_t) ,typeof( __mavlink_set_altitude_t) ,typeof( __mavlink_request_data_stream_t) ,typeof( __mavlink_request_dynamic_gyro_calibration_t ) ,typeof( __mavlink_request_static_calibration_t ) ,typeof( __mavlink_manual_control_t) ,typeof( __mavlink_rc_channels_override_t) ,null ,null ,typeof( __mavlink_global_position_int_t) ,typeof( __mavlink_vfr_hud_t) ,typeof( __mavlink_command_t) ,typeof( __mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_optical_flow_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_object_detection_event_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_sensor_offsets_t) ,typeof( __mavlink_set_mag_offsets_t) ,typeof( __mavlink_meminfo_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_debug_vect_t) ,typeof( __mavlink_named_value_float_t) ,typeof( __mavlink_named_value_int_t) ,typeof( __mavlink_statustext_t) ,typeof( __mavlink_debug_t) ,null ,}; } } diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs index dce000b17e..864b322f5d 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs @@ -7,159 +7,7 @@ namespace ArdupilotMega { partial class MAVLink { - public enum MAV_CLASS - { - MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything - MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch - MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu - MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com - MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org - MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints - MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands - MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set - MAV_CLASS_NONE = 8, ///< No valid autopilot - MAV_CLASS_NB ///< Number of autopilot classes - }; - - public enum MAV_ACTION - { - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACTION_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_DELAY_BEFORE_COMMAND = 36, - MAV_ACTION_ASCEND_AT_RATE = 37, - MAV_ACTION_CHANGE_MODE = 38, - MAV_ACTION_LOITER_MAX_TURNS = 39, - MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, - MAV_ACTION_NB ///< Number of MAV actions - }; - - public enum MAV_MODE - { - MAV_MODE_UNINIT = 0, ///< System is in undefined state - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back - }; - - public enum MAV_STATE - { - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_HILSIM, - MAV_STATE_POWEROFF - }; - - public enum MAV_NAV - { - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST, - MAV_NAV_LOITER, - MAV_NAV_FREE_DRIFT - }; - - public enum MAV_TYPE - { - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6, - MAV_AIRSHIP = 7, - MAV_FREE_BALLOON = 8, - MAV_ROCKET = 9, - UGV_GROUND_ROVER = 10, - UGV_SURFACE_SHIP = 11 - }; - - public enum MAV_AUTOPILOT_TYPE - { - MAV_AUTOPILOT_GENERIC = 0, - MAV_AUTOPILOT_PIXHAWK = 1, - MAV_AUTOPILOT_SLUGS = 2, - MAV_AUTOPILOT_ARDUPILOTMEGA = 3, - MAV_AUTOPILOT_NONE = 4 - }; - - public enum MAV_COMPONENT - { - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_CAMERA, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_IMU_2 = 201, - MAV_COMP_ID_IMU_3 = 202, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250 - }; - - public enum MAV_FRAME - { - MAV_FRAME_GLOBAL = 0, - MAV_FRAME_LOCAL = 1, - MAV_FRAME_MISSION = 2, - MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, - MAV_FRAME_LOCAL_ENU = 4 - }; + } } diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 451e8acea8..8d2d26a0e7 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -76,7 +76,13 @@ namespace ArdupilotMega //System.Threading.Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US"); //System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US"); - srtm.datadirectory = @"C:\srtm"; + srtm.datadirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "srtm"; + + georefimage temp = new georefimage(); + + //temp.dowork(141); + + //return; var t = Type.GetType("Mono.Runtime"); MAC = (t != null); @@ -155,6 +161,15 @@ namespace ArdupilotMega } catch (Exception e) { MessageBox.Show("A Major error has occured : " + e.ToString()); this.Close(); } + GCSViews.FlightData.myhud.Refresh(); + GCSViews.FlightData.myhud.Refresh(); + GCSViews.FlightData.myhud.Refresh(); + + if (GCSViews.FlightData.myhud.huddrawtime > 1000) + { + MessageBox.Show("The HUD draw time is above 1 seconds. Please update your graphics card driver."); + } + changeunits(); try @@ -941,7 +956,7 @@ namespace ArdupilotMega if (heatbeatsend.Second != DateTime.Now.Second) { - Console.WriteLine("remote lost {0}", cs.packetdrop); + Console.WriteLine("remote lost {0}", cs.packetdropremote); MAVLink.__mavlink_heartbeat_t htb = new MAVLink.__mavlink_heartbeat_t(); @@ -954,10 +969,13 @@ namespace ArdupilotMega } // data loss warning - if (speechenable && talk != null && (DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10) + if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10) { - if (MainV2.talk.State == SynthesizerState.Ready) - MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds"); + if (speechenable && talk != null) { + if (MainV2.talk.State == SynthesizerState.Ready) + MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds"); + } + MainV2.cs.linkqualitygcs = 0; } //Console.WriteLine(comPort.BaseStream.BytesToRead); diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs index 4ab6aec1c7..8f357122d7 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs @@ -431,13 +431,14 @@ namespace ArdupilotMega // bar moves to 100 % in this step progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f); - Application.DoEvents(); + progressBar1.Refresh(); + //Application.DoEvents(); byte[] packet = mine.readPacket(); string text = ""; mine.DebugPacket(packet, ref text); - sw.Write(text); + sw.Write(mine.lastlogread +" "+text); } sw.Close(); diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index d4f74b5646..2ee1a13020 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.68")] +[assembly: AssemblyFileVersion("1.0.69")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt b/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt index 2bba4ab6e7..011946774a 100644 --- a/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt +++ b/Tools/ArdupilotMegaPlanner/Resources/MAVParam.txt @@ -1,4 +1,4 @@ -== MAVLink Parameters == +== MAVLink Parameters == (this is a copy fo the wiki page FYI) This is a list of all the user-modifiable MAVLink parameters and what they do. You can modify them via the MAVLink parameters window in any compatible GCS, such as the Mission Planner, HK GCS or !QGroundControl. @@ -213,9 +213,9 @@ It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some ||SYSID_SW_MREV|| || ||0|| || ||Description coming soon|| ||SYSID_SW_TYPE|| || ||0|| || ||Description coming soon|| ||THR_SLEWRATE||0||100||0||1||1||THROTTLE_SLEW_RATE - 0 = Disabled, otherwise it limits throttle movement rate. Units are % per second. This is a test feature and may go away.|| -||FLTMODE1||0||20||1||1|| ||FLIGHT_MODE_1 - Mode switch setting 1 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE2||0||20||1||1|| ||FLIGHT_MODE_2 - Mode switch setting 2 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE3||0||20||1||1|| ||FLIGHT_MODE_3 - Mode switch setting 3 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE4||0||20||1||1|| ||FLIGHT_MODE_4 - Mode switch setting 4 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE5||0||20||1||1|| ||FLIGHT_MODE_5 - Mode switch setting 5 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| -||FLTMODE6||0||20||1||1|| ||FLIGHT_MODE_6 - Mode switch setting 6 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 2 = Acro, 3 = Simple, 4 = Auto, 5 = Guided, 6 = Loiter, 7 = RTL|| \ No newline at end of file +||FLTMODE1||0||20||1||1|| ||FLIGHT_MODE_1 - Mode switch setting 1 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE2||0||20||1||1|| ||FLIGHT_MODE_2 - Mode switch setting 2 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE3||0||20||1||1|| ||FLIGHT_MODE_3 - Mode switch setting 3 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE4||0||20||1||1|| ||FLIGHT_MODE_4 - Mode switch setting 4 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE5||0||20||1||1|| ||FLIGHT_MODE_5 - Mode switch setting 5 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| +||FLTMODE6||0||20||1||1|| ||FLIGHT_MODE_6 - Mode switch setting 6 - APM: 0 = Manual, 2 = Stabilize, 5 - Fly-By-Wire-A, 6 = Fly-By-Wire-B, 7 = Fly-By-Wire-C, 10 = Auto - Mission, 11 = RTL, 12 = Loiter, 13 = Take-off, 14 = Land, 15= Guided; ACM2: 0 = Stabilize, 1 = Acro, 2 = Alt Hold, 3 = Auto, 4 = Guided, 5 = Loiter, 6 = RTL|| \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs index d8cf085621..243c6d9816 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs @@ -32,22 +32,11 @@ System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Setup)); this.tabControl1 = new System.Windows.Forms.TabControl(); this.tabReset = new System.Windows.Forms.TabPage(); - this.BUT_reset = new ArdupilotMega.MyButton(); this.tabRadioIn = new System.Windows.Forms.TabPage(); this.CHK_revch3 = new System.Windows.Forms.CheckBox(); this.CHK_revch4 = new System.Windows.Forms.CheckBox(); this.CHK_revch2 = new System.Windows.Forms.CheckBox(); this.CHK_revch1 = new System.Windows.Forms.CheckBox(); - this.BUT_Calibrateradio = new ArdupilotMega.MyButton(); - this.BAR8 = new ArdupilotMega.HorizontalProgressBar2(); - this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components); - this.BAR7 = new ArdupilotMega.HorizontalProgressBar2(); - this.BAR6 = new ArdupilotMega.HorizontalProgressBar2(); - this.BAR5 = new ArdupilotMega.HorizontalProgressBar2(); - this.BARpitch = new ArdupilotMega.VerticalProgressBar2(); - this.BARthrottle = new ArdupilotMega.VerticalProgressBar2(); - this.BARyaw = new ArdupilotMega.HorizontalProgressBar2(); - this.BARroll = new ArdupilotMega.HorizontalProgressBar2(); this.tabModes = new System.Windows.Forms.TabPage(); this.label14 = new System.Windows.Forms.Label(); this.LBL_flightmodepwm = new System.Windows.Forms.Label(); @@ -71,7 +60,6 @@ this.CMB_fmode2 = new System.Windows.Forms.ComboBox(); this.label1 = new System.Windows.Forms.Label(); this.CMB_fmode1 = new System.Windows.Forms.ComboBox(); - this.BUT_SaveModes = new ArdupilotMega.MyButton(); this.tabHardware = new System.Windows.Forms.TabPage(); this.linkLabelmagdec = new System.Windows.Forms.LinkLabel(); this.label106 = new System.Windows.Forms.Label(); @@ -90,7 +78,6 @@ this.pictureBox1 = new System.Windows.Forms.PictureBox(); this.tabArducopter = new System.Windows.Forms.TabPage(); this.label28 = new System.Windows.Forms.Label(); - this.BUT_levelac2 = new ArdupilotMega.MyButton(); this.label16 = new System.Windows.Forms.Label(); this.label15 = new System.Windows.Forms.Label(); this.pictureBoxQuadX = new System.Windows.Forms.PictureBox(); @@ -119,6 +106,20 @@ this.HS2_REV = new System.Windows.Forms.CheckBox(); this.HS1_REV = new System.Windows.Forms.CheckBox(); this.label17 = new System.Windows.Forms.Label(); + this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); + this.BUT_reset = new ArdupilotMega.MyButton(); + this.BUT_Calibrateradio = new ArdupilotMega.MyButton(); + this.BAR8 = new ArdupilotMega.HorizontalProgressBar2(); + this.currentStateBindingSource = new System.Windows.Forms.BindingSource(this.components); + this.BAR7 = new ArdupilotMega.HorizontalProgressBar2(); + this.BAR6 = new ArdupilotMega.HorizontalProgressBar2(); + this.BAR5 = new ArdupilotMega.HorizontalProgressBar2(); + this.BARpitch = new ArdupilotMega.VerticalProgressBar2(); + this.BARthrottle = new ArdupilotMega.VerticalProgressBar2(); + this.BARyaw = new ArdupilotMega.HorizontalProgressBar2(); + this.BARroll = new ArdupilotMega.HorizontalProgressBar2(); + this.BUT_SaveModes = new ArdupilotMega.MyButton(); + this.BUT_levelac2 = new ArdupilotMega.MyButton(); this.BUT_saveheliconfig = new ArdupilotMega.MyButton(); this.BUT_0collective = new ArdupilotMega.MyButton(); this.HS4 = new ArdupilotMega.VerticalProgressBar2(); @@ -128,11 +129,15 @@ this.HS2_TRIM = new ArdupilotMega.MyTrackBar(); this.HS1_TRIM = new ArdupilotMega.MyTrackBar(); this.Gservoloc = new AGaugeApp.AGauge(); - this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); + this.CB_simple1 = new System.Windows.Forms.CheckBox(); + this.CB_simple2 = new System.Windows.Forms.CheckBox(); + this.CB_simple3 = new System.Windows.Forms.CheckBox(); + this.CB_simple4 = new System.Windows.Forms.CheckBox(); + this.CB_simple5 = new System.Windows.Forms.CheckBox(); + this.CB_simple6 = new System.Windows.Forms.CheckBox(); this.tabControl1.SuspendLayout(); this.tabReset.SuspendLayout(); this.tabRadioIn.SuspendLayout(); - ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit(); this.tabModes.SuspendLayout(); this.tabHardware.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox4)).BeginInit(); @@ -143,6 +148,7 @@ ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuadX)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).BeginInit(); this.tabHeli.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).BeginInit(); @@ -169,14 +175,6 @@ this.tabReset.Name = "tabReset"; this.tabReset.UseVisualStyleBackColor = true; // - // BUT_reset - // - resources.ApplyResources(this.BUT_reset, "BUT_reset"); - this.BUT_reset.Name = "BUT_reset"; - this.BUT_reset.Tag = ""; - this.BUT_reset.UseVisualStyleBackColor = true; - this.BUT_reset.Click += new System.EventHandler(this.BUT_reset_Click); - // // tabRadioIn // this.tabRadioIn.Controls.Add(this.CHK_revch3); @@ -224,139 +222,14 @@ this.CHK_revch1.UseVisualStyleBackColor = true; this.CHK_revch1.CheckedChanged += new System.EventHandler(this.CHK_revch1_CheckedChanged); // - // BUT_Calibrateradio - // - resources.ApplyResources(this.BUT_Calibrateradio, "BUT_Calibrateradio"); - this.BUT_Calibrateradio.Name = "BUT_Calibrateradio"; - this.BUT_Calibrateradio.UseVisualStyleBackColor = true; - this.BUT_Calibrateradio.Click += new System.EventHandler(this.BUT_Calibrateradio_Click); - // - // BAR8 - // - this.BAR8.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR8.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR8.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch8in", true)); - this.BAR8.Label = "Radio 8"; - resources.ApplyResources(this.BAR8, "BAR8"); - this.BAR8.Maximum = 2200; - this.BAR8.maxline = 0; - this.BAR8.Minimum = 800; - this.BAR8.minline = 0; - this.BAR8.Name = "BAR8"; - this.BAR8.Value = 1500; - this.BAR8.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // currentStateBindingSource - // - this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState); - // - // BAR7 - // - this.BAR7.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR7.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR7.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch7in", true)); - this.BAR7.Label = "Radio 7"; - resources.ApplyResources(this.BAR7, "BAR7"); - this.BAR7.Maximum = 2200; - this.BAR7.maxline = 0; - this.BAR7.Minimum = 800; - this.BAR7.minline = 0; - this.BAR7.Name = "BAR7"; - this.BAR7.Value = 1500; - this.BAR7.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BAR6 - // - this.BAR6.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR6.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR6.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch6in", true)); - this.BAR6.Label = "Radio 6"; - resources.ApplyResources(this.BAR6, "BAR6"); - this.BAR6.Maximum = 2200; - this.BAR6.maxline = 0; - this.BAR6.Minimum = 800; - this.BAR6.minline = 0; - this.BAR6.Name = "BAR6"; - this.BAR6.Value = 1500; - this.BAR6.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BAR5 - // - this.BAR5.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BAR5.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BAR5.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch5in", true)); - this.BAR5.Label = "Radio 5"; - resources.ApplyResources(this.BAR5, "BAR5"); - this.BAR5.Maximum = 2200; - this.BAR5.maxline = 0; - this.BAR5.Minimum = 800; - this.BAR5.minline = 0; - this.BAR5.Name = "BAR5"; - this.BAR5.Value = 1500; - this.BAR5.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BARpitch - // - this.BARpitch.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BARpitch.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARpitch.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch2in", true)); - this.BARpitch.Label = "Pitch"; - resources.ApplyResources(this.BARpitch, "BARpitch"); - this.BARpitch.Maximum = 2200; - this.BARpitch.maxline = 0; - this.BARpitch.Minimum = 800; - this.BARpitch.minline = 0; - this.BARpitch.Name = "BARpitch"; - this.BARpitch.Value = 1500; - this.BARpitch.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BARthrottle - // - this.BARthrottle.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(67)))), ((int)(((byte)(68)))), ((int)(((byte)(69))))); - this.BARthrottle.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARthrottle.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch3in", true)); - this.BARthrottle.Label = "Throttle"; - resources.ApplyResources(this.BARthrottle, "BARthrottle"); - this.BARthrottle.Maximum = 2200; - this.BARthrottle.maxline = 0; - this.BARthrottle.Minimum = 800; - this.BARthrottle.minline = 0; - this.BARthrottle.Name = "BARthrottle"; - this.BARthrottle.Value = 1000; - this.BARthrottle.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(148)))), ((int)(((byte)(193)))), ((int)(((byte)(31))))); - // - // BARyaw - // - this.BARyaw.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BARyaw.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARyaw.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch4in", true)); - this.BARyaw.Label = "Yaw"; - resources.ApplyResources(this.BARyaw, "BARyaw"); - this.BARyaw.Maximum = 2200; - this.BARyaw.maxline = 0; - this.BARyaw.Minimum = 800; - this.BARyaw.minline = 0; - this.BARyaw.Name = "BARyaw"; - this.BARyaw.Value = 1500; - this.BARyaw.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // - // BARroll - // - this.BARroll.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); - this.BARroll.BorderColor = System.Drawing.SystemColors.ActiveBorder; - this.BARroll.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch1in", true)); - this.BARroll.Label = "Roll"; - resources.ApplyResources(this.BARroll, "BARroll"); - this.BARroll.Maximum = 2200; - this.BARroll.maxline = 0; - this.BARroll.Minimum = 800; - this.BARroll.minline = 0; - this.BARroll.Name = "BARroll"; - this.BARroll.Value = 1500; - this.BARroll.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); - // // tabModes // + this.tabModes.Controls.Add(this.CB_simple6); + this.tabModes.Controls.Add(this.CB_simple5); + this.tabModes.Controls.Add(this.CB_simple4); + this.tabModes.Controls.Add(this.CB_simple3); + this.tabModes.Controls.Add(this.CB_simple2); + this.tabModes.Controls.Add(this.CB_simple1); this.tabModes.Controls.Add(this.label14); this.tabModes.Controls.Add(this.LBL_flightmodepwm); this.tabModes.Controls.Add(this.label13); @@ -519,13 +392,6 @@ resources.ApplyResources(this.CMB_fmode1, "CMB_fmode1"); this.CMB_fmode1.Name = "CMB_fmode1"; // - // BUT_SaveModes - // - resources.ApplyResources(this.BUT_SaveModes, "BUT_SaveModes"); - this.BUT_SaveModes.Name = "BUT_SaveModes"; - this.BUT_SaveModes.UseVisualStyleBackColor = true; - this.BUT_SaveModes.Click += new System.EventHandler(this.BUT_SaveModes_Click); - // // tabHardware // this.tabHardware.BackColor = System.Drawing.Color.DarkRed; @@ -661,11 +527,11 @@ // tabArducopter // this.tabArducopter.Controls.Add(this.label28); - this.tabArducopter.Controls.Add(this.BUT_levelac2); this.tabArducopter.Controls.Add(this.label16); this.tabArducopter.Controls.Add(this.label15); this.tabArducopter.Controls.Add(this.pictureBoxQuadX); this.tabArducopter.Controls.Add(this.pictureBoxQuad); + this.tabArducopter.Controls.Add(this.BUT_levelac2); resources.ApplyResources(this.tabArducopter, "tabArducopter"); this.tabArducopter.Name = "tabArducopter"; this.tabArducopter.UseVisualStyleBackColor = true; @@ -675,13 +541,6 @@ resources.ApplyResources(this.label28, "label28"); this.label28.Name = "label28"; // - // BUT_levelac2 - // - resources.ApplyResources(this.BUT_levelac2, "BUT_levelac2"); - this.BUT_levelac2.Name = "BUT_levelac2"; - this.BUT_levelac2.UseVisualStyleBackColor = true; - this.BUT_levelac2.Click += new System.EventHandler(this.BUT_levelac2_Click); - // // label16 // resources.ApplyResources(this.label16, "label16"); @@ -879,6 +738,159 @@ resources.ApplyResources(this.label17, "label17"); this.label17.Name = "label17"; // + // BUT_reset + // + resources.ApplyResources(this.BUT_reset, "BUT_reset"); + this.BUT_reset.Name = "BUT_reset"; + this.BUT_reset.Tag = ""; + this.BUT_reset.UseVisualStyleBackColor = true; + this.BUT_reset.Click += new System.EventHandler(this.BUT_reset_Click); + // + // BUT_Calibrateradio + // + resources.ApplyResources(this.BUT_Calibrateradio, "BUT_Calibrateradio"); + this.BUT_Calibrateradio.Name = "BUT_Calibrateradio"; + this.BUT_Calibrateradio.UseVisualStyleBackColor = true; + this.BUT_Calibrateradio.Click += new System.EventHandler(this.BUT_Calibrateradio_Click); + // + // BAR8 + // + this.BAR8.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR8.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR8.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch8in", true)); + this.BAR8.Label = "Radio 8"; + resources.ApplyResources(this.BAR8, "BAR8"); + this.BAR8.Maximum = 2200; + this.BAR8.maxline = 0; + this.BAR8.Minimum = 800; + this.BAR8.minline = 0; + this.BAR8.Name = "BAR8"; + this.BAR8.Value = 1500; + this.BAR8.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // currentStateBindingSource + // + this.currentStateBindingSource.DataSource = typeof(ArdupilotMega.CurrentState); + // + // BAR7 + // + this.BAR7.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR7.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR7.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch7in", true)); + this.BAR7.Label = "Radio 7"; + resources.ApplyResources(this.BAR7, "BAR7"); + this.BAR7.Maximum = 2200; + this.BAR7.maxline = 0; + this.BAR7.Minimum = 800; + this.BAR7.minline = 0; + this.BAR7.Name = "BAR7"; + this.BAR7.Value = 1500; + this.BAR7.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BAR6 + // + this.BAR6.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR6.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR6.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch6in", true)); + this.BAR6.Label = "Radio 6"; + resources.ApplyResources(this.BAR6, "BAR6"); + this.BAR6.Maximum = 2200; + this.BAR6.maxline = 0; + this.BAR6.Minimum = 800; + this.BAR6.minline = 0; + this.BAR6.Name = "BAR6"; + this.BAR6.Value = 1500; + this.BAR6.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BAR5 + // + this.BAR5.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BAR5.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BAR5.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch5in", true)); + this.BAR5.Label = "Radio 5"; + resources.ApplyResources(this.BAR5, "BAR5"); + this.BAR5.Maximum = 2200; + this.BAR5.maxline = 0; + this.BAR5.Minimum = 800; + this.BAR5.minline = 0; + this.BAR5.Name = "BAR5"; + this.BAR5.Value = 1500; + this.BAR5.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BARpitch + // + this.BARpitch.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BARpitch.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARpitch.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch2in", true)); + this.BARpitch.Label = "Pitch"; + resources.ApplyResources(this.BARpitch, "BARpitch"); + this.BARpitch.Maximum = 2200; + this.BARpitch.maxline = 0; + this.BARpitch.Minimum = 800; + this.BARpitch.minline = 0; + this.BARpitch.Name = "BARpitch"; + this.BARpitch.Value = 1500; + this.BARpitch.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BARthrottle + // + this.BARthrottle.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(67)))), ((int)(((byte)(68)))), ((int)(((byte)(69))))); + this.BARthrottle.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARthrottle.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch3in", true)); + this.BARthrottle.Label = "Throttle"; + resources.ApplyResources(this.BARthrottle, "BARthrottle"); + this.BARthrottle.Maximum = 2200; + this.BARthrottle.maxline = 0; + this.BARthrottle.Minimum = 800; + this.BARthrottle.minline = 0; + this.BARthrottle.Name = "BARthrottle"; + this.BARthrottle.Value = 1000; + this.BARthrottle.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(148)))), ((int)(((byte)(193)))), ((int)(((byte)(31))))); + // + // BARyaw + // + this.BARyaw.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BARyaw.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARyaw.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch4in", true)); + this.BARyaw.Label = "Yaw"; + resources.ApplyResources(this.BARyaw, "BARyaw"); + this.BARyaw.Maximum = 2200; + this.BARyaw.maxline = 0; + this.BARyaw.Minimum = 800; + this.BARyaw.minline = 0; + this.BARyaw.Name = "BARyaw"; + this.BARyaw.Value = 1500; + this.BARyaw.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BARroll + // + this.BARroll.BackgroundColor = System.Drawing.Color.FromArgb(((int)(((byte)(20)))), ((int)(((byte)(20)))), ((int)(((byte)(255))))); + this.BARroll.BorderColor = System.Drawing.SystemColors.ActiveBorder; + this.BARroll.DataBindings.Add(new System.Windows.Forms.Binding("Value", this.currentStateBindingSource, "ch1in", true)); + this.BARroll.Label = "Roll"; + resources.ApplyResources(this.BARroll, "BARroll"); + this.BARroll.Maximum = 2200; + this.BARroll.maxline = 0; + this.BARroll.Minimum = 800; + this.BARroll.minline = 0; + this.BARroll.Name = "BARroll"; + this.BARroll.Value = 1500; + this.BARroll.ValueColor = System.Drawing.Color.FromArgb(((int)(((byte)(255)))), ((int)(((byte)(0)))), ((int)(((byte)(255))))); + // + // BUT_SaveModes + // + resources.ApplyResources(this.BUT_SaveModes, "BUT_SaveModes"); + this.BUT_SaveModes.Name = "BUT_SaveModes"; + this.BUT_SaveModes.UseVisualStyleBackColor = true; + this.BUT_SaveModes.Click += new System.EventHandler(this.BUT_SaveModes_Click); + // + // BUT_levelac2 + // + resources.ApplyResources(this.BUT_levelac2, "BUT_levelac2"); + this.BUT_levelac2.Name = "BUT_levelac2"; + this.BUT_levelac2.UseVisualStyleBackColor = true; + this.BUT_levelac2.Click += new System.EventHandler(this.BUT_levelac2_Click); + // // BUT_saveheliconfig // resources.ApplyResources(this.BUT_saveheliconfig, "BUT_saveheliconfig"); @@ -925,8 +937,8 @@ // // HS4_TRIM // - this.HS4_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS4_TRIM, "HS4_TRIM"); + this.HS4_TRIM.LargeChange = 1000; this.HS4_TRIM.Maximum = 2000D; this.HS4_TRIM.Minimum = 1000D; this.HS4_TRIM.Name = "HS4_TRIM"; @@ -937,8 +949,8 @@ // // HS3_TRIM // - this.HS3_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS3_TRIM, "HS3_TRIM"); + this.HS3_TRIM.LargeChange = 1000; this.HS3_TRIM.Maximum = 2000D; this.HS3_TRIM.Minimum = 1000D; this.HS3_TRIM.Name = "HS3_TRIM"; @@ -949,8 +961,8 @@ // // HS2_TRIM // - this.HS2_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS2_TRIM, "HS2_TRIM"); + this.HS2_TRIM.LargeChange = 1000; this.HS2_TRIM.Maximum = 2000D; this.HS2_TRIM.Minimum = 1000D; this.HS2_TRIM.Name = "HS2_TRIM"; @@ -961,8 +973,8 @@ // // HS1_TRIM // - this.HS1_TRIM.LargeChange = 1000; resources.ApplyResources(this.HS1_TRIM, "HS1_TRIM"); + this.HS1_TRIM.LargeChange = 1000; this.HS1_TRIM.Maximum = 2000D; this.HS1_TRIM.Minimum = 1000D; this.HS1_TRIM.Name = "HS1_TRIM"; @@ -1114,6 +1126,42 @@ this.Gservoloc.Value2 = 180F; this.Gservoloc.Value3 = 0F; // + // CB_simple1 + // + resources.ApplyResources(this.CB_simple1, "CB_simple1"); + this.CB_simple1.Name = "CB_simple1"; + this.CB_simple1.UseVisualStyleBackColor = true; + // + // CB_simple2 + // + resources.ApplyResources(this.CB_simple2, "CB_simple2"); + this.CB_simple2.Name = "CB_simple2"; + this.CB_simple2.UseVisualStyleBackColor = true; + // + // CB_simple3 + // + resources.ApplyResources(this.CB_simple3, "CB_simple3"); + this.CB_simple3.Name = "CB_simple3"; + this.CB_simple3.UseVisualStyleBackColor = true; + // + // CB_simple4 + // + resources.ApplyResources(this.CB_simple4, "CB_simple4"); + this.CB_simple4.Name = "CB_simple4"; + this.CB_simple4.UseVisualStyleBackColor = true; + // + // CB_simple5 + // + resources.ApplyResources(this.CB_simple5, "CB_simple5"); + this.CB_simple5.Name = "CB_simple5"; + this.CB_simple5.UseVisualStyleBackColor = true; + // + // CB_simple6 + // + resources.ApplyResources(this.CB_simple6, "CB_simple6"); + this.CB_simple6.Name = "CB_simple6"; + this.CB_simple6.UseVisualStyleBackColor = true; + // // Setup // resources.ApplyResources(this, "$this"); @@ -1126,7 +1174,6 @@ this.tabReset.ResumeLayout(false); this.tabRadioIn.ResumeLayout(false); this.tabRadioIn.PerformLayout(); - ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit(); this.tabModes.ResumeLayout(false); this.tabModes.PerformLayout(); this.tabHardware.ResumeLayout(false); @@ -1141,6 +1188,7 @@ ((System.ComponentModel.ISupportInitialize)(this.pictureBoxQuad)).EndInit(); this.tabHeli.ResumeLayout(false); this.tabHeli.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)(this.currentStateBindingSource)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.HS4_TRIM)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.HS3_TRIM)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.HS2_TRIM)).EndInit(); @@ -1250,6 +1298,12 @@ private System.Windows.Forms.CheckBox CHK_revch4; private System.Windows.Forms.CheckBox CHK_revch2; private System.Windows.Forms.CheckBox CHK_revch1; + private System.Windows.Forms.CheckBox CB_simple6; + private System.Windows.Forms.CheckBox CB_simple5; + private System.Windows.Forms.CheckBox CB_simple4; + private System.Windows.Forms.CheckBox CB_simple3; + private System.Windows.Forms.CheckBox CB_simple2; + private System.Windows.Forms.CheckBox CB_simple1; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 17c296b914..b2b9ad463c 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -252,6 +252,13 @@ namespace ArdupilotMega.Setup { if (MainV2.cs.firmware == MainV2.Firmwares.ArduPlane) // APM { + CB_simple1.Visible = false; + CB_simple2.Visible = false; + CB_simple3.Visible = false; + CB_simple4.Visible = false; + CB_simple5.Visible = false; + CB_simple6.Visible = false; + CMB_fmode1.Items.Clear(); CMB_fmode2.Items.Clear(); CMB_fmode3.Items.Clear(); @@ -410,6 +417,10 @@ namespace ArdupilotMega.Setup MainV2.comPort.setParam("FLTMODE4", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode4.Text)); MainV2.comPort.setParam("FLTMODE5", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode5.Text)); MainV2.comPort.setParam("FLTMODE6", (float)(int)Enum.Parse(typeof(Common.ac2modes), CMB_fmode6.Text)); + + float value = (float)(CB_simple1.Checked ? 1 : 0) + (CB_simple2.Checked ? 1 << 1 : 0) + (CB_simple3.Checked ? 1 <<2 : 0) + + (CB_simple4.Checked ? 1 << 3 : 0) + (CB_simple5.Checked ? 1 << 4 : 0) + (CB_simple6.Checked ? 1 << 5 : 0); + MainV2.comPort.setParam("SIMPLE", value); } } catch { MessageBox.Show("Failed to set Flight modes"); } diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx index be2a79628f..fdcafe6fac 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx @@ -117,20 +117,6 @@ System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - 214, 161 - - - 195, 23 - - - - 0 - - - Reset APM Hardware to Default - BUT_reset @@ -143,12 +129,14 @@ 0 + 4, 22 666, 393 + 4 @@ -167,10 +155,1780 @@ 0 + + CHK_revch3 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 0 + + + CHK_revch4 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 1 + + + CHK_revch2 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 2 + + + CHK_revch1 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabRadioIn + + + 3 + + + BUT_Calibrateradio + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 4 + + + BAR8 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 5 + + + BAR7 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 6 + + + BAR6 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 7 + + + BAR5 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 8 + + + BARpitch + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 9 + + + BARthrottle + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 10 + + + BARyaw + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 11 + + + BARroll + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 12 + + + 4, 22 + + + + 3, 3, 3, 3 + + + 666, 393 + + + 0 + + + Radio Input + + + tabRadioIn + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 1 + + + True + + + NoControl + + + 372, 235 + + + 87, 17 + + + 119 + + + Simple Mode + + + CB_simple6 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 0 + + + True + + + NoControl + + + 373, 208 + + + 87, 17 + + + 118 + + + Simple Mode + + + CB_simple5 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 1 + + + True + + + NoControl + + + 372, 181 + + + 87, 17 + + + 117 + + + Simple Mode + + + CB_simple4 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 2 + + + True + + + NoControl + + + 373, 154 + + + 87, 17 + + + 116 + + + Simple Mode + + + CB_simple3 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 3 + + + True + + + NoControl + + + 372, 127 + + + 87, 17 + + + 115 + + + Simple Mode + + + CB_simple2 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 4 + + + True + + + 373, 101 + + + 87, 17 + + + 114 + + + Simple Mode + + + CB_simple1 + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 5 + + + True + + + NoControl + + + 242, 67 + + + 74, 13 + + + 113 + + + Current PWM: + + + label14 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 6 + + + True + + + NoControl + + + 322, 67 + + + 13, 13 + + + 112 + + + 0 + + + LBL_flightmodepwm + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 7 + + + True + + + NoControl + + + 242, 50 + + + 74, 13 + + + 111 + + + Current Mode: + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 8 + + + True + + + NoControl + + + 322, 50 + + + 42, 13 + + + 110 + + + Manual + + + lbl_currentmode + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 9 + + + True + + + NoControl + + + 483, 101 + + + 76, 13 + + + 109 + + + PWM 0 - 1230 + + + False + + + label12 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 10 + + + True + + + NoControl + + + 483, 236 + + + 70, 13 + + + 108 + + + PWM 1750 + + + + False + + + label11 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 11 + + + True + + + NoControl + + + 483, 209 + + + 94, 13 + + + 107 + + + PWM 1621 - 1749 + + + False + + + label10 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 12 + + + True + + + NoControl + + + 483, 182 + + + 94, 13 + + + 106 + + + PWM 1491 - 1620 + + + False + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 13 + + + True + + + NoControl + + + 483, 155 + + + 94, 13 + + + 105 + + + PWM 1361 - 1490 + + + False + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 14 + + + True + + + NoControl + + + 483, 128 + + + 94, 13 + + + 104 + + + PWM 1231 - 1360 + + + False + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 15 + + + True + + + NoControl + + + 168, 236 + + + 71, 13 + + + 11 + + + Flight Mode 6 + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 16 + + + 245, 233 + + + 121, 21 + + + 10 + + + CMB_fmode6 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 17 + + + True + + + NoControl + + + 168, 209 + + + 71, 13 + + + 9 + + + Flight Mode 5 + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 18 + + + 245, 206 + + + 121, 21 + + + 8 + + + CMB_fmode5 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 19 + + + True + + + NoControl + + + 168, 182 + + + 71, 13 + + + 7 + + + Flight Mode 4 + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 20 + + + 245, 179 + + + 121, 21 + + + 6 + + + CMB_fmode4 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 21 + + + True + + + NoControl + + + 168, 155 + + + 71, 13 + + + 5 + + + Flight Mode 3 + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 22 + + + 245, 152 + + + 121, 21 + + + 4 + + + CMB_fmode3 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 23 + + + True + + + NoControl + + + 168, 128 + + + 71, 13 + + + 3 + + + Flight Mode 2 + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 24 + + + 245, 125 + + + 121, 21 + + + 2 + + + CMB_fmode2 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 25 + + + True + + + NoControl + + + 168, 101 + + + 71, 13 + + + 1 + + + Flight Mode 1 + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 26 + + + 245, 98 + + + 121, 21 + + + 0 + + + CMB_fmode1 + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabModes + + + 27 + + + NoControl + + + 245, 260 + + + 121, 23 + + + 103 + + + Save Modes + + + BUT_SaveModes + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabModes + + + 28 + + + 4, 22 + + + 666, 393 + + + 3 + + + Modes + + + tabModes + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 2 + + + linkLabelmagdec + + + System.Windows.Forms.LinkLabel, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 0 + + + label106 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 1 + + + label105 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 2 + + + TXT_battcapacity + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 3 + + + CMB_batmontype + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 4 + + + label100 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 5 + + + TXT_declination + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 6 + + + CHK_enableairspeed + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 7 + + + CHK_enablesonar + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 8 + + + CHK_enablebattmon + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 9 + + + CHK_enablecompass + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 10 + + + pictureBox4 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 11 + + + pictureBox3 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 12 + + + pictureBox2 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 13 + + + pictureBox1 + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHardware + + + 14 + + + 4, 22 + + + 3, 3, 3, 3 + + + 666, 393 + + + 1 + + + Hardware + + + tabHardware + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 3 + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 0 + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 1 + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 2 + + + pictureBoxQuadX + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 3 + + + pictureBoxQuad + + + System.Windows.Forms.PictureBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabArducopter + + + 4 + + + BUT_levelac2 + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabArducopter + + + 5 + + + 4, 22 + + + 666, 393 + + + 2 + + + ArduCopter2 + + + tabArducopter + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 4 + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 0 + + + GYR_GAIN_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 1 + + + GYR_ENABLE_ + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 2 + + + label26 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 3 + + + PIT_MAX_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 4 + + + label25 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 5 + + + ROL_MAX_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 6 + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 7 + + + COL_MID_ + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 8 + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 9 + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 10 + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 11 + + + HS4_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 12 + + + label20 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 13 + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 14 + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 15 + + + SV3_POS_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 16 + + + SV2_POS_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 17 + + + SV1_POS_ + + + System.Windows.Forms.TextBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 18 + + + HS3_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 19 + + + HS2_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 20 + + + HS1_REV + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 21 + + + label17 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabHeli + + + 22 + + + BUT_saveheliconfig + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 23 + + + BUT_0collective + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 24 + + + HS4 + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 25 + + + HS3 + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 26 + + + HS4_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 27 + + + HS3_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 28 + + + HS2_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 29 + + + HS1_TRIM + + + ArdupilotMega.MyTrackBar, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 30 + + + Gservoloc + + + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabHeli + + + 31 + + + 4, 22 + + + 666, 393 + + + 5 + + + AC2 Heli - BETA + + + tabHeli + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + tabControl1 + + + 5 + + + Fill + + + 0, 0 + + + 674, 419 + + + 93 + + + tabControl1 + + + System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + + 0 + True - NoControl @@ -288,837 +2046,12 @@ 3 - - 482, 340 - - - 134, 23 - - - 102 - - - Calibrate Radio - - - BUT_Calibrateradio - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 4 - - - 17, 17 - - - 446, 240 - - - 170, 25 - - - 101 - - - BAR8 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 5 - - - 446, 185 - - - 170, 25 - - - 100 - - - BAR7 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 6 - - - 446, 130 - - - 170, 25 - - - 99 - - - BAR6 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 7 - - - 446, 75 - - - 170, 25 - - - 98 - - - BAR5 - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 8 - - - 143, 61 - - - 47, 211 - - - 96 - - - BARpitch - - - ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 9 - - - 359, 61 - - - 47, 211 - - - 95 - - - BARthrottle - - - ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 10 - - - 21, 304 - - - 288, 23 - - - 94 - - - BARyaw - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 11 - - - 21, 6 - - - 288, 23 - - - 93 - - - BARroll - - - ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabRadioIn - - - 12 - - - 4, 22 - - - 3, 3, 3, 3 - - - 666, 393 - - - 0 - - - Radio Input - - - tabRadioIn - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 1 - - - True - - - 242, 67 - - - 74, 13 - - - 113 - - - Current PWM: - - - label14 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 0 - - - True - - - 322, 67 - - - 13, 13 - - - 112 - - - 0 - - - LBL_flightmodepwm - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 1 - - - True - - - 242, 50 - - - 74, 13 - - - 111 - - - Current Mode: - - - label13 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 2 - - - True - - - 322, 50 - - - 42, 13 - - - 110 - - - Manual - - - lbl_currentmode - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 3 - - - True - - - 372, 101 - - - 76, 13 - - - 109 - - - PWM 0 - 1230 - - - label12 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 4 - - - True - - - 372, 236 - - - 70, 13 - - - 108 - - - PWM 1750 + - - - label11 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 5 - - - True - - - 372, 209 - - - 94, 13 - - - 107 - - - PWM 1621 - 1749 - - - label10 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 6 - - - True - - - 372, 182 - - - 94, 13 - - - 106 - - - PWM 1491 - 1620 - - - label9 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 7 - - - True - - - 372, 155 - - - 94, 13 - - - 105 - - - PWM 1361 - 1490 - - - label8 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 8 - - - True - - - 372, 128 - - - 94, 13 - - - 104 - - - PWM 1231 - 1360 - - - label7 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 9 - - - True - - - 168, 236 - - - 71, 13 - - - 11 - - - Flight Mode 6 - - - label6 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 10 - - - 245, 233 - - - 121, 21 - - - 10 - - - CMB_fmode6 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 11 - - - True - - - 168, 209 - - - 71, 13 - - - 9 - - - Flight Mode 5 - - - label5 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 12 - - - 245, 206 - - - 121, 21 - - - 8 - - - CMB_fmode5 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 13 - - - True - - - 168, 182 - - - 71, 13 - - - 7 - - - Flight Mode 4 - - - label4 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 14 - - - 245, 179 - - - 121, 21 - - - 6 - - - CMB_fmode4 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 15 - - - True - - - 168, 155 - - - 71, 13 - - - 5 - - - Flight Mode 3 - - - label3 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 16 - - - 245, 152 - - - 121, 21 - - - 4 - - - CMB_fmode3 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 17 - - - True - - - 168, 128 - - - 71, 13 - - - 3 - - - Flight Mode 2 - - - label2 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 18 - - - 245, 125 - - - 121, 21 - - - 2 - - - CMB_fmode2 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 19 - - - True - - - 168, 101 - - - 71, 13 - - - 1 - - - Flight Mode 1 - - - label1 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 20 - - - 245, 98 - - - 121, 21 - - - 0 - - - CMB_fmode1 - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabModes - - - 21 - - - 245, 260 - - - 121, 23 - - - 103 - - - Save Modes - - - BUT_SaveModes - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabModes - - - 22 - - - 4, 22 - - - 666, 393 - - - 3 - - - Modes - - - tabModes - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 2 - True + + NoControl + 390, 80 @@ -1281,6 +2214,9 @@ 5 + + 214, 17 + 383, 57 @@ -1290,9 +2226,6 @@ 20 - - 214, 17 - Magnetic Declination (-20.0 to 20.0) eg 2° 3' W is -2.3 @@ -1419,6 +2352,9 @@ Zoom + + NoControl + 78, 268 @@ -1443,6 +2379,9 @@ Zoom + + NoControl + 78, 187 @@ -1467,6 +2406,9 @@ Zoom + + NoControl + 78, 106 @@ -1494,6 +2436,9 @@ + + NoControl + @@ -1518,36 +2463,12 @@ 14 - - 4, 22 - - - 3, 3, 3, 3 - - - 666, 393 - - - 1 - - - Hardware - - - tabHardware - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 3 - True + + NoControl + 217, 38 @@ -1572,33 +2493,12 @@ 0 - - 274, 67 - - - 75, 23 - - - 8 - - - Level - - - BUT_levelac2 - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - tabArducopter - - - 1 - True + + NoControl + 217, 333 @@ -1622,11 +2522,14 @@ will work with hexa's etc tabArducopter - 2 + 1 True + + NoControl + 260, 124 @@ -1649,7 +2552,7 @@ will work with hexa's etc tabArducopter - 3 + 2 NoControl @@ -1676,7 +2579,7 @@ will work with hexa's etc tabArducopter - 4 + 3 NoControl @@ -1703,35 +2606,14 @@ will work with hexa's etc tabArducopter - 5 - - - 4, 22 - - - 666, 393 - - - 2 - - - ArduCopter2 - - - tabArducopter - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - 4 True + + NoControl + 552, 222 @@ -1783,6 +2665,9 @@ will work with hexa's etc True + + NoControl + 540, 202 @@ -1810,6 +2695,9 @@ will work with hexa's etc True + + NoControl + 563, 94 @@ -1861,6 +2749,9 @@ will work with hexa's etc True + + NoControl + 563, 50 @@ -1912,6 +2803,9 @@ will work with hexa's etc True + + NoControl + 537, 170 @@ -1939,6 +2833,9 @@ will work with hexa's etc True + + NoControl + 603, 170 @@ -1966,6 +2863,9 @@ will work with hexa's etc True + + NoControl + 440, 23 @@ -1993,6 +2893,9 @@ will work with hexa's etc True + + NoControl + 290, 23 @@ -2020,6 +2923,9 @@ will work with hexa's etc True + + NoControl + 178, 23 @@ -2047,6 +2953,9 @@ will work with hexa's etc True + + NoControl + 181, 148 @@ -2074,6 +2983,9 @@ will work with hexa's etc True + + NoControl + 13, 258 @@ -2101,6 +3013,9 @@ will work with hexa's etc True + + NoControl + 13, 232 @@ -2128,6 +3043,9 @@ will work with hexa's etc True + + NoControl + 13, 206 @@ -2227,6 +3145,9 @@ will work with hexa's etc True + + NoControl + 181, 125 @@ -2254,6 +3175,9 @@ will work with hexa's etc True + + NoControl + 181, 102 @@ -2281,6 +3205,9 @@ will work with hexa's etc True + + NoControl + 181, 79 @@ -2308,6 +3235,9 @@ will work with hexa's etc True + + NoControl + 38, 23 @@ -2332,6 +3262,267 @@ will work with hexa's etc 22 + + 214, 17 + + + NoControl + + + 214, 161 + + + 195, 23 + + + 0 + + + Reset APM Hardware to Default + + + BUT_reset + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabReset + + + 0 + + + NoControl + + + 482, 340 + + + 134, 23 + + + 102 + + + Calibrate Radio + + + BUT_Calibrateradio + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 4 + + + 17, 17 + + + 446, 240 + + + 170, 25 + + + 101 + + + BAR8 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 5 + + + 17, 17 + + + 446, 185 + + + 170, 25 + + + 100 + + + BAR7 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 6 + + + 446, 130 + + + 170, 25 + + + 99 + + + BAR6 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 7 + + + 446, 75 + + + 170, 25 + + + 98 + + + BAR5 + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 8 + + + 143, 61 + + + 47, 211 + + + 96 + + + BARpitch + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 9 + + + 359, 61 + + + 47, 211 + + + 95 + + + BARthrottle + + + ArdupilotMega.VerticalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 10 + + + 21, 304 + + + 288, 23 + + + 94 + + + BARyaw + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 11 + + + 21, 6 + + + 288, 23 + + + 93 + + + BARroll + + + ArdupilotMega.HorizontalProgressBar2, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabRadioIn + + + 12 + + + NoControl + + + 274, 67 + + + 75, 23 + + + 8 + + + Level + + + BUT_levelac2 + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + tabArducopter + + + 5 + + + NoControl + 430, 307 @@ -2356,6 +3547,9 @@ will work with hexa's etc 23 + + NoControl + 540, 144 @@ -2422,6 +3616,9 @@ will work with hexa's etc 26 + + NoControl + 276, 203 @@ -2443,6 +3640,9 @@ will work with hexa's etc 27 + + NoControl + 276, 152 @@ -2464,6 +3664,9 @@ will work with hexa's etc 28 + + NoControl + 276, 101 @@ -2485,6 +3688,9 @@ will work with hexa's etc 29 + + NoControl + 276, 50 @@ -2536,54 +3742,6 @@ will work with hexa's etc 31 - - 4, 22 - - - 666, 393 - - - 5 - - - AC2 Heli - BETA - - - tabHeli - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - tabControl1 - - - 5 - - - Fill - - - 0, 0 - - - 674, 419 - - - 93 - - - tabControl1 - - - System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - $this - - - 0 - True @@ -2593,21 +3751,24 @@ will work with hexa's etc 674, 419 + + NoControl + APMSetup - - currentStateBindingSource - - - System.Windows.Forms.BindingSource, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - toolTip1 System.Windows.Forms.ToolTip, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + currentStateBindingSource + + + System.Windows.Forms.BindingSource, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + Setup diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx index 8c4784c6d9..1ecdd308f8 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/Configuration.resx @@ -192,6 +192,2421 @@ Top, Bottom, Left, Right + + 111, 82 + + + 78, 20 + + + 11 + + + THR_FS_VALUE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 0 + + + NoControl + + + 6, 86 + + + 50, 13 + + + 12 + + + FS Value + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + THR_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 2 + + + NoControl + + + 6, 63 + + + 27, 13 + + + 13 + + + Max + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + THR_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 4 + + + NoControl + + + 6, 40 + + + 24, 13 + + + 14 + + + Min + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_THROTTLE + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 6 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 15 + + + Cruise + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 7 + + + 405, 217 + + + 195, 108 + + + 0 + + + Throttle 0-100% + + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 0 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSPD_RATIO + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 0 + + + NoControl + + + 6, 87 + + + 32, 13 + + + 1 + + + Ratio + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSPD_FBW_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 2 + + + NoControl + + + 6, 59 + + + 53, 13 + + + 3 + + + FBW max + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSPD_FBW_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 4 + + + NoControl + + + 6, 40 + + + 50, 13 + + + 5 + + + FBW min + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + TRIM_ARSPD_CM + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 6 + + + NoControl + + + 6, 17 + + + 64, 13 + + + 6 + + + Cruise + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 7 + + + 406, 325 + + + 195, 108 + + + 1 + + + Airspeed m/s + + + groupBox1 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + LIM_PITCH_MIN + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 0 + + + NoControl + + + 6, 63 + + + 51, 13 + + + 10 + + + Pitch Min + + + label39 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 1 + + + 111, 36 + + + 78, 20 + + + 7 + + + LIM_PITCH_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 2 + + + NoControl + + + 6, 40 + + + 54, 13 + + + 11 + + + Pitch Max + + + label38 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 5 + + + LIM_ROLL_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 4 + + + NoControl + + + 6, 17 + + + 55, 13 + + + 12 + + + Bank Max + + + label37 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 5 + + + 205, 325 + + + 195, 108 + + + 2 + + + Navigation Angles + + + groupBox2 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 2 + + + 111, 36 + + + 78, 20 + + + 7 + + + XTRK_ANGLE_CD + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 0 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 8 + + + Entry Angle + + + label79 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 1 + + + 111, 13 + + + 78, 20 + + + 5 + + + XTRK_GAIN_SC + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 2 + + + NoControl + + + 6, 17 + + + 52, 13 + + + 9 + + + Gain (cm) + + + label80 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 3 + + + 4, 325 + + + 195, 108 + + + 3 + + + Xtrack Pids + + + groupBox15 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 3 + + + 111, 13 + + + 78, 20 + + + 13 + + + KFF_PTCH2THR + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 0 + + + NoControl + + + 6, 17 + + + 36, 13 + + + 14 + + + P to T + + + label83 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + KFF_RDDRMIX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 2 + + + NoControl + + + 6, 63 + + + 61, 13 + + + 15 + + + Rudder Mix + + + label78 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + KFF_PTCHCOMP + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 4 + + + NoControl + + + 6, 40 + + + 61, 13 + + + 16 + + + Pitch Comp + + + label81 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 5 + + + 205, 217 + + + 195, 108 + + + 4 + + + Other Mix's + + + groupBox16 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 4 + + + 111, 82 + + + 78, 20 + + + 11 + + + ENRGY2THR_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label73 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + ENRGY2THR_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label74 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + ENRGY2THR_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label75 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + ENRGY2THR_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label76 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 7 + + + 4, 217 + + + 195, 108 + + + 5 + + + Energy/Alt Pid + + + groupBox14 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 5 + + + 111, 82 + + + 78, 20 + + + 0 + + + ALT2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label69 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ALT2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label70 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ALT2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label71 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ALT2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label72 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 7 + + + 406, 109 + + + 195, 108 + + + 6 + + + Nav Pitch Alt Pid + + + groupBox13 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 6 + + + 111, 82 + + + 78, 20 + + + 0 + + + ARSP2PTCH_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 1 + + + INT_MAX + + + label65 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 1 + + + 111, 59 + + + 78, 20 + + + 2 + + + ARSP2PTCH_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 3 + + + D + + + label66 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 3 + + + 111, 36 + + + 78, 20 + + + 4 + + + ARSP2PTCH_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label67 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 5 + + + 111, 13 + + + 78, 20 + + + 6 + + + ARSP2PTCH_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 7 + + + P + + + label68 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 7 + + + 205, 109 + + + 195, 108 + + + 7 + + + Nav Pitch AS Pid + + + groupBox12 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 7 + + + 111, 82 + + + 78, 20 + + + 11 + + + HDNG2RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label61 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + HDNG2RLL_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label62 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + HDNG2RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label63 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + HDNG2RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label64 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 7 + + + 4, 109 + + + 195, 108 + + + 8 + + + Nav Roll Pid + + + groupBox11 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 8 + + + 111, 82 + + + 78, 20 + + + 11 + + + YW2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label57 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + YW2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label58 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + YW2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label59 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + YW2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label60 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 7 + + + 406, 1 + + + 195, 108 + + + 9 + + + Servo Yaw Pid + + + groupBox10 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 9 + + + 111, 82 + + + 78, 20 + + + 11 + + + PTCH2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label53 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + PTCH2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label54 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + PTCH2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label55 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + PTCH2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label56 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 7 + + + 205, 1 + + + 195, 108 + + + 10 + + + Servo Pitch Pid + + + groupBox9 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 10 + + + 111, 82 + + + 78, 20 + + + 11 + + + RLL2SRV_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 0 + + + NoControl + + + 6, 86 + + + 54, 13 + + + 12 + + + INT_MAX + + + label49 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 1 + + + 111, 59 + + + 78, 20 + + + 9 + + + RLL2SRV_D + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 2 + + + NoControl + + + 6, 63 + + + 15, 13 + + + 13 + + + D + + + label50 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 3 + + + 111, 36 + + + 78, 20 + + + 7 + + + RLL2SRV_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label51 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 5 + + + 111, 13 + + + 78, 20 + + + 5 + + + RLL2SRV_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 6 + + + NoControl + + + 6, 17 + + + 14, 13 + + + 15 + + + P + + + label52 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 7 + + + 4, 1 + + + 195, 108 + + + 11 + + + Servo Roll Pid + + + groupBox8 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAPM2 + + + 11 + + + 4, 22 + + + 0, 0, 0, 0 + + + 722, 434 + + + 0 + + + APM 2.x + TabAPM2 @@ -204,6 +2619,1824 @@ 0 + + True + + + 3, 198 + + + 154, 17 + + + 13 + + + Lock Pitch and Roll Values + + + CHK_lockrollpitch + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 0 + + + 80, 86 + + + 78, 20 + + + 16 + + + WP_SPEED_MAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 0 + + + NoControl + + + 6, 89 + + + 54, 13 + + + 17 + + + m/s + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + NAV_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + NAV_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + NAV_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 7 + + + 534, 107 + + + 170, 108 + + + 0 + + + Nav WP + + + groupBox4 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 1 + + + 80, 86 + + + 78, 20 + + + 18 + + + XTRK_ANGLE_CD1 + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 0 + + + NoControl + + + 6, 89 + + + 82, 13 + + + 19 + + + Error Max + + + label10 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 1 + + + 80, 63 + + + 78, 20 + + + 11 + + + XTRACK_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 2 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label11 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 3 + + + 80, 37 + + + 78, 20 + + + 7 + + + XTRACK_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 4 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label17 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 5 + + + 80, 13 + + + 78, 20 + + + 5 + + + XTRACK_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 6 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 7 + + + 182, 241 + + + 170, 110 + + + 2 + + + Crosstrack Correction + + + groupBox6 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 2 + + + 80, 63 + + + 78, 20 + + + 11 + + + THR_HOLD_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + THR_HOLD_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + THR_HOLD_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 5 + + + 6, 241 + + + 170, 110 + + + 3 + + + Altitude Hold + + + groupBox7 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 3 + + + 80, 61 + + + 78, 20 + + + 11 + + + HLD_LAT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 0 + + + NoControl + + + 6, 64 + + + 65, 13 + + + 12 + + + IMAX + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + HLD_LAT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label30 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + HLD_LAT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label31 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 5 + + + 531, 6 + + + 170, 95 + + + 6 + + + Loiter + + + groupBox19 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 4 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label32 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label34 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label35 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 5 + + + 358, 6 + + + 170, 95 + + + 7 + + + Stabilize Yaw + + + groupBox20 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 5 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label36 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label41 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label42 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 5 + + + 182, 6 + + + 170, 95 + + + 8 + + + Stabilize Pitch + + + groupBox21 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 6 + + + 80, 63 + + + 78, 20 + + + 11 + + + STB_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 12 + + + IMAX + + + label43 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 1 + + + 80, 37 + + + 78, 20 + + + 7 + + + STB_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 14 + + + I + + + label45 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 3 + + + 80, 13 + + + 78, 20 + + + 5 + + + STB_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 15 + + + P + + + label46 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 5 + + + 6, 6 + + + 170, 95 + + + 9 + + + Stabilize Roll + + + groupBox22 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 7 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_YAW_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label47 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_YAW_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label77 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_YAW_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label82 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 5 + + + 358, 107 + + + 170, 91 + + + 10 + + + Rate Yaw + + + groupBox23 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 8 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_PIT_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 0 + + + NoControl + + + 6, 66 + + + 65, 13 + + + 1 + + + IMAX + + + label84 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_PIT_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label86 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_PIT_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label87 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 5 + + + 182, 107 + + + 170, 91 + + + 11 + + + Rate Pitch + + + groupBox24 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 9 + + + 80, 63 + + + 78, 20 + + + 0 + + + RATE_RLL_IMAX + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 0 + + + NoControl + + + 6, 66 + + + 68, 13 + + + 1 + + + IMAX + + + label88 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 1 + + + 80, 37 + + + 78, 20 + + + 4 + + + RATE_RLL_I + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 2 + + + NoControl + + + 6, 40 + + + 10, 13 + + + 5 + + + I + + + label90 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 3 + + + 80, 13 + + + 78, 20 + + + 6 + + + RATE_RLL_P + + + System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 4 + + + NoControl + + + 6, 16 + + + 14, 13 + + + 7 + + + P + + + label91 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 5 + + + 6, 107 + + + 170, 91 + + + 12 + + + Rate Roll + + + groupBox25 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC2 + + + 10 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 1 + + + AC2 + TabAC2 @@ -216,6 +4449,1072 @@ 1 + + NoControl + + + 30, 277 + + + 61, 13 + + + 37 + + + Waypoints + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 0 + + + NoControl + + + 139, 276 + + + 177, 17 + + + 38 + + + Load Waypoints on connect? + + + CHK_loadwponconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 1 + + + NoControl + + + 30, 252 + + + 103, 18 + + + 36 + + + Track Length + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 2 + + + 139, 250 + + + 67, 20 + + + 35 + + + 17, 17 + + + On the Flight Data Tab + + + NUM_tracklength + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 3 + + + NoControl + + + 579, 67 + + + 102, 17 + + + 34 + + + Alt Warning + + + CHK_speechaltwarning + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 4 + + + NoControl + + + 30, 228 + + + 61, 13 + + + 0 + + + APM Reset + + + label108 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 5 + + + NoControl + + + 139, 227 + + + 163, 17 + + + 1 + + + Reset APM on USB Connect + + + CHK_resetapmonconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 6 + + + Bottom, Left + + + NoControl + + + 33, 411 + + + 144, 17 + + + 2 + + + Mavlink Message Debug + + + CHK_mavdebug + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 7 + + + NoControl + + + 590, 203 + + + 22, 13 + + + 3 + + + RC + + + label107 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 8 + + + 0 + + + 1 + + + 3 + + + 10 + + + 621, 200 + + + 80, 21 + + + 4 + + + CMB_raterc + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 9 + + + NoControl + + + 425, 203 + + + 69, 13 + + + 5 + + + Mode/Status + + + label104 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 10 + + + NoControl + + + 280, 203 + + + 44, 13 + + + 6 + + + Position + + + label103 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 11 + + + NoControl + + + 136, 203 + + + 43, 13 + + + 7 + + + Attitude + + + label102 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 12 + + + NoControl + + + 27, 203 + + + 84, 13 + + + 8 + + + Telemetry Rates + + + label101 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 13 + + + 0 + + + 1 + + + 3 + + + 10 + + + 499, 200 + + + 80, 21 + + + 9 + + + CMB_ratestatus + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 14 + + + 0 + + + 1 + + + 3 + + + 10 + + + 334, 200 + + + 80, 21 + + + 10 + + + CMB_rateposition + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 15 + + + 0 + + + 1 + + + 3 + + + 10 + + + 188, 200 + + + 80, 21 + + + 11 + + + CMB_rateattitude + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 16 + + + NoControl + + + 283, 168 + + + 402, 13 + + + 12 + + + NOTE: The Configuration Tab will NOT display these units, as those are raw values. + + + + label99 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 17 + + + NoControl + + + 30, 176 + + + 65, 13 + + + 13 + + + Speed Units + + + label98 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 18 + + + NoControl + + + 30, 149 + + + 52, 13 + + + 14 + + + Dist Units + + + label97 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 19 + + + 139, 173 + + + 138, 21 + + + 15 + + + CMB_speedunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 20 + + + 139, 146 + + + 138, 21 + + + 16 + + + CMB_distunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 21 + + + NoControl + + + 30, 122 + + + 45, 13 + + + 17 + + + Joystick + + + label96 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 22 + + + NoControl + + + 30, 71 + + + 44, 13 + + + 18 + + + Speech + + + label95 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 23 + + + NoControl + + + 471, 67 + + + 102, 17 + + + 19 + + + Battery Warning + + + CHK_speechbattery + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 24 + + + NoControl + + + 378, 67 + + + 87, 17 + + + 20 + + + Time Interval + + + CHK_speechcustom + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 25 + + + NoControl + + + 322, 67 + + + 56, 17 + + + 21 + + + Mode + + + CHK_speechmode + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 26 + + + NoControl + + + 245, 67 + + + 71, 17 + + + 22 + + + Waypoint + + + CHK_speechwaypoint + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 27 + + + NoControl + + + 30, 47 + + + 57, 13 + + + 23 + + + OSD Color + + + label94 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 28 + + + 139, 40 + + + 138, 21 + + + 24 + + + CMB_osdcolor + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 29 + + + 139, 90 + + + 138, 21 + + + 25 + + + CMB_language + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 30 + + + NoControl + + + 30, 94 + + + 69, 13 + + + 26 + + + UI Language + + + label93 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 31 + + + NoControl + + + 139, 67 + + + 99, 17 + + + 27 + + + Enable Speech + + + CHK_enablespeech + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 32 + + + NoControl + + + 552, 15 + + + 125, 17 + + + 28 + + + Enable HUD Overlay + + + CHK_hudshow + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 33 + + + NoControl + + + 30, 16 + + + 100, 23 + + + 29 + + + Video Device + + + label92 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 34 + + + 139, 13 + + + 245, 21 + + + 30 + + + CMB_videosources + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 35 + + + NoControl + + + 139, 117 + + + 99, 23 + + + 31 + + + Joystick Setup + + + BUT_Joystick + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 36 + + + NoControl + + + 471, 11 + + + 75, 23 + + + 32 + + + Stop + + + BUT_videostop + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 37 + + + NoControl + + + 390, 11 + + + 75, 23 + + + 33 + + + Start + + + BUT_videostart + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c + + + TabPlanner + + + 38 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 2 + + + Planner + TabPlanner @@ -228,6 +5527,18 @@ 2 + + 4, 22 + + + 722, 434 + + + 3 + + + Setup + TabSetup @@ -270,5389 +5581,6 @@ 2 - - groupBox3 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 0 - - - groupBox1 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 1 - - - groupBox2 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 2 - - - groupBox15 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 3 - - - groupBox16 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 4 - - - groupBox14 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 5 - - - groupBox13 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 6 - - - groupBox12 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 7 - - - groupBox11 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 8 - - - groupBox10 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 9 - - - groupBox9 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 10 - - - groupBox8 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAPM2 - - - 11 - - - 4, 22 - - - 0, 0, 0, 0 - - - 722, 434 - - - 0 - - - APM 2.x - - - THR_FS_VALUE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 0 - - - label5 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 1 - - - THR_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 2 - - - label6 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 3 - - - THR_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 4 - - - label7 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 5 - - - TRIM_THROTTLE - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 6 - - - label8 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox3 - - - 7 - - - 405, 217 - - - 195, 108 - - - 0 - - - Throttle 0-100% - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 50, 13 - - - 12 - - - FS Value - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 27, 13 - - - 13 - - - Max - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 24, 13 - - - 14 - - - Min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 15 - - - Cruise - - - ARSPD_RATIO - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 0 - - - label1 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 1 - - - ARSPD_FBW_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 2 - - - label2 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 3 - - - ARSPD_FBW_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 4 - - - label3 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 5 - - - TRIM_ARSPD_CM - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 6 - - - label4 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox1 - - - 7 - - - 406, 325 - - - 195, 108 - - - 1 - - - Airspeed m/s - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 87 - - - 32, 13 - - - 1 - - - Ratio - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 59 - - - 53, 13 - - - 3 - - - FBW max - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 50, 13 - - - 5 - - - FBW min - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 64, 13 - - - 6 - - - Cruise * 100 - - - LIM_PITCH_MIN - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 0 - - - label39 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 1 - - - LIM_PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 2 - - - label38 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 3 - - - LIM_ROLL_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 4 - - - label37 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox2 - - - 5 - - - 205, 325 - - - 195, 108 - - - 2 - - - Navigation Angles *100 - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 51, 13 - - - 10 - - - Pitch Min - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 54, 13 - - - 11 - - - Pitch Max - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 55, 13 - - - 12 - - - Bank Max - - - XTRK_ANGLE_CD - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 0 - - - label79 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 1 - - - XTRK_GAIN_SC - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 2 - - - label80 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox15 - - - 3 - - - 4, 325 - - - 195, 108 - - - 3 - - - Xtrack Pids - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 8 - - - Entry Angle - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 52, 13 - - - 9 - - - Gain (cm) - - - KFF_PTCH2THR - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 0 - - - label83 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 1 - - - KFF_RDDRMIX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 2 - - - label78 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 3 - - - KFF_PTCHCOMP - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 4 - - - label81 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox16 - - - 5 - - - 205, 217 - - - 195, 108 - - - 4 - - - Other Mix's - - - 111, 13 - - - 78, 20 - - - 13 - - - NoControl - - - 6, 17 - - - 36, 13 - - - 14 - - - P to T - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 61, 13 - - - 15 - - - Rudder Mix - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 61, 13 - - - 16 - - - Pitch Comp - - - ENRGY2THR_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 0 - - - label73 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 1 - - - ENRGY2THR_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 2 - - - label74 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 3 - - - ENRGY2THR_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 4 - - - label75 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 5 - - - ENRGY2THR_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 6 - - - label76 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox14 - - - 7 - - - 4, 217 - - - 195, 108 - - - 5 - - - Energy/Alt Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - ALT2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 0 - - - label69 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 1 - - - ALT2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 2 - - - label70 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 3 - - - ALT2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 4 - - - label71 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 5 - - - ALT2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 6 - - - label72 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox13 - - - 7 - - - 406, 109 - - - 195, 108 - - - 6 - - - Nav Pitch Alt Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - ARSP2PTCH_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 0 - - - label65 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 1 - - - ARSP2PTCH_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 2 - - - label66 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 3 - - - ARSP2PTCH_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 4 - - - label67 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 5 - - - ARSP2PTCH_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 6 - - - label68 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox12 - - - 7 - - - 205, 109 - - - 195, 108 - - - 7 - - - Nav Pitch AS Pid - - - 111, 82 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 1 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 2 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 3 - - - D - - - 111, 36 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 111, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 7 - - - P - - - HDNG2RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 0 - - - label61 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 1 - - - HDNG2RLL_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 2 - - - label62 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 3 - - - HDNG2RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 4 - - - label63 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 5 - - - HDNG2RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 6 - - - label64 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox11 - - - 7 - - - 4, 109 - - - 195, 108 - - - 8 - - - Nav Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - YW2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 0 - - - label57 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 1 - - - YW2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 2 - - - label58 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 3 - - - YW2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 4 - - - label59 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 5 - - - YW2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 6 - - - label60 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox10 - - - 7 - - - 406, 1 - - - 195, 108 - - - 9 - - - Servo Yaw Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - PTCH2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 0 - - - label53 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 1 - - - PTCH2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 2 - - - label54 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 3 - - - PTCH2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 4 - - - label55 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 5 - - - PTCH2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 6 - - - label56 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox9 - - - 7 - - - 205, 1 - - - 195, 108 - - - 10 - - - Servo Pitch Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - RLL2SRV_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 0 - - - label49 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 1 - - - RLL2SRV_D - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 2 - - - label50 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 3 - - - RLL2SRV_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 4 - - - label51 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 5 - - - RLL2SRV_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 6 - - - label52 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox8 - - - 7 - - - 4, 1 - - - 195, 108 - - - 11 - - - Servo Roll Pid - - - 111, 82 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 86 - - - 54, 13 - - - 12 - - - INT_MAX - - - 111, 59 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 63 - - - 15, 13 - - - 13 - - - D - - - 111, 36 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 111, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 17 - - - 14, 13 - - - 15 - - - P - - - CHK_lockrollpitch - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 0 - - - groupBox4 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 1 - - - groupBox6 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 2 - - - groupBox7 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 3 - - - groupBox18 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 4 - - - groupBox19 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 5 - - - groupBox20 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 6 - - - groupBox21 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 7 - - - groupBox22 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 8 - - - groupBox23 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 9 - - - groupBox24 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 10 - - - groupBox25 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC2 - - - 11 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 1 - - - AC2 - - - True - - - 3, 198 - - - 154, 17 - - - 13 - - - Lock Pitch and Roll Values - - - WP_SPEED_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 0 - - - label9 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 1 - - - NAV_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 2 - - - label13 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 3 - - - NAV_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 4 - - - label15 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 5 - - - NAV_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 6 - - - label16 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 7 - - - 534, 107 - - - 170, 108 - - - 0 - - - Nav WP - - - 80, 86 - - - 78, 20 - - - 16 - - - NoControl - - - 6, 89 - - - 54, 13 - - - 17 - - - cm/s - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - XTRK_ANGLE_CD1 - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 0 - - - label10 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 1 - - - XTRACK_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 2 - - - label11 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 3 - - - XTRACK_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 4 - - - label17 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 5 - - - XTRACK_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 6 - - - label18 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 7 - - - 182, 241 - - - 170, 110 - - - 2 - - - Crosstrack Correction - - - 80, 86 - - - 78, 20 - - - 18 - - - NoControl - - - 6, 89 - - - 82, 13 - - - 19 - - - Error Max * 100 - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - THR_HOLD_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 0 - - - label19 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 1 - - - THR_HOLD_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 2 - - - label21 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 3 - - - THR_HOLD_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 4 - - - label22 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 5 - - - 6, 241 - - - 170, 110 - - - 3 - - - Altitude Hold - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - PITCH_MAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 0 - - - label27 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox18 - - - 1 - - - 525, 241 - - - 176, 110 - - - 5 - - - Other Mix's - - - 94, 17 - - - 78, 20 - - - 9 - - - NoControl - - - 6, 20 - - - 82, 13 - - - 10 - - - Pitch Max * 100 - - - HLD_LAT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 0 - - - label28 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 1 - - - HLD_LAT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 2 - - - label30 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 3 - - - HLD_LAT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 4 - - - label31 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 5 - - - 531, 6 - - - 170, 95 - - - 6 - - - Loiter - - - 80, 61 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 64 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 0 - - - label32 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 1 - - - STB_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 2 - - - label34 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 3 - - - STB_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 4 - - - label35 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 5 - - - 358, 6 - - - 170, 95 - - - 7 - - - Stabilize Yaw - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 0 - - - label36 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 1 - - - STB_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 2 - - - label41 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 3 - - - STB_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 4 - - - label42 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 5 - - - 182, 6 - - - 170, 95 - - - 8 - - - Stabilize Pitch - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - STB_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 0 - - - label43 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 1 - - - STB_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 2 - - - label45 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 3 - - - STB_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 4 - - - label46 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 5 - - - 6, 6 - - - 170, 95 - - - 9 - - - Stabilize Roll - - - 80, 63 - - - 78, 20 - - - 11 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 12 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 7 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 14 - - - I - - - 80, 13 - - - 78, 20 - - - 5 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 15 - - - P - - - RATE_YAW_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 0 - - - label47 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 1 - - - RATE_YAW_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 2 - - - label77 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 3 - - - RATE_YAW_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 4 - - - label82 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 5 - - - 358, 107 - - - 170, 91 - - - 10 - - - Rate Yaw - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_PIT_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 0 - - - label84 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 1 - - - RATE_PIT_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 2 - - - label86 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 3 - - - RATE_PIT_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 4 - - - label87 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 5 - - - 182, 107 - - - 170, 91 - - - 11 - - - Rate Pitch - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 65, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - RATE_RLL_IMAX - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 0 - - - label88 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 1 - - - RATE_RLL_I - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 2 - - - label90 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 3 - - - RATE_RLL_P - - - System.Windows.Forms.DomainUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 4 - - - label91 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 5 - - - 6, 107 - - - 170, 91 - - - 12 - - - Rate Roll - - - 80, 63 - - - 78, 20 - - - 0 - - - NoControl - - - 6, 66 - - - 68, 13 - - - 1 - - - IMAX * 100 - - - 80, 37 - - - 78, 20 - - - 4 - - - NoControl - - - 6, 40 - - - 10, 13 - - - 5 - - - I - - - 80, 13 - - - 78, 20 - - - 6 - - - NoControl - - - 6, 16 - - - 14, 13 - - - 7 - - - P - - - label24 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 0 - - - CHK_loadwponconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 1 - - - label23 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 2 - - - NUM_tracklength - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 3 - - - CHK_speechaltwarning - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 4 - - - label108 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 5 - - - CHK_resetapmonconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 6 - - - CHK_mavdebug - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 7 - - - label107 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 8 - - - CMB_raterc - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 9 - - - label104 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 10 - - - label103 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 11 - - - label102 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 12 - - - label101 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 13 - - - CMB_ratestatus - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 14 - - - CMB_rateposition - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 15 - - - CMB_rateattitude - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 16 - - - label99 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 17 - - - label98 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 18 - - - label97 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 19 - - - CMB_speedunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 20 - - - CMB_distunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 21 - - - label96 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 22 - - - label95 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 23 - - - CHK_speechbattery - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 24 - - - CHK_speechcustom - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 25 - - - CHK_speechmode - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 26 - - - CHK_speechwaypoint - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 27 - - - label94 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 28 - - - CMB_osdcolor - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 29 - - - CMB_language - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 30 - - - label93 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 31 - - - CHK_enablespeech - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 32 - - - CHK_hudshow - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 33 - - - label92 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 34 - - - CMB_videosources - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 35 - - - BUT_Joystick - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 36 - - - BUT_videostop - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 37 - - - BUT_videostart - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=ff91852278f5092c - - - TabPlanner - - - 38 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 2 - - - Planner - - - NoControl - - - 30, 277 - - - 61, 13 - - - 37 - - - Waypoints - - - NoControl - - - 139, 276 - - - 177, 17 - - - 38 - - - Load Waypoints on connect? - - - NoControl - - - 30, 252 - - - 103, 18 - - - 36 - - - Track Length - - - 17, 17 - - - 139, 250 - - - 67, 20 - - - 35 - - - On the Flight Data Tab - - - NoControl - - - 579, 67 - - - 102, 17 - - - 34 - - - Alt Warning - - - NoControl - - - 30, 228 - - - 61, 13 - - - 0 - - - APM Reset - - - NoControl - - - 139, 227 - - - 163, 17 - - - 1 - - - Reset APM on USB Connect - - - Bottom, Left - - - NoControl - - - 33, 411 - - - 144, 17 - - - 2 - - - Mavlink Message Debug - - - NoControl - - - 590, 203 - - - 22, 13 - - - 3 - - - RC - - - 0 - - - 1 - - - 3 - - - 10 - - - 621, 200 - - - 80, 21 - - - 4 - - - NoControl - - - 425, 203 - - - 69, 13 - - - 5 - - - Mode/Status - - - NoControl - - - 280, 203 - - - 44, 13 - - - 6 - - - Position - - - NoControl - - - 136, 203 - - - 43, 13 - - - 7 - - - Attitude - - - NoControl - - - 27, 203 - - - 84, 13 - - - 8 - - - Telemetry Rates - - - 0 - - - 1 - - - 3 - - - 10 - - - 499, 200 - - - 80, 21 - - - 9 - - - 0 - - - 1 - - - 3 - - - 10 - - - 334, 200 - - - 80, 21 - - - 10 - - - 0 - - - 1 - - - 3 - - - 10 - - - 188, 200 - - - 80, 21 - - - 11 - - - NoControl - - - 283, 168 - - - 402, 13 - - - 12 - - - NOTE: The Configuration Tab will NOT display these units, as those are raw values. - - - - NoControl - - - 30, 176 - - - 65, 13 - - - 13 - - - Speed Units - - - NoControl - - - 30, 149 - - - 52, 13 - - - 14 - - - Dist Units - - - 139, 173 - - - 138, 21 - - - 15 - - - 139, 146 - - - 138, 21 - - - 16 - - - NoControl - - - 30, 122 - - - 45, 13 - - - 17 - - - Joystick - - - NoControl - - - 30, 71 - - - 44, 13 - - - 18 - - - Speech - - - NoControl - - - 471, 67 - - - 102, 17 - - - 19 - - - Battery Warning - - - NoControl - - - 378, 67 - - - 87, 17 - - - 20 - - - Time Interval - - - NoControl - - - 322, 67 - - - 56, 17 - - - 21 - - - Mode - - - NoControl - - - 245, 67 - - - 71, 17 - - - 22 - - - Waypoint - - - NoControl - - - 30, 47 - - - 57, 13 - - - 23 - - - OSD Color - - - 139, 40 - - - 138, 21 - - - 24 - - - 139, 90 - - - 138, 21 - - - 25 - - - NoControl - - - 30, 94 - - - 69, 13 - - - 26 - - - UI Language - - - NoControl - - - 139, 67 - - - 99, 17 - - - 27 - - - Enable Speech - - - NoControl - - - 552, 15 - - - 125, 17 - - - 28 - - - Enable HUD Overlay - - - NoControl - - - 30, 16 - - - 100, 23 - - - 29 - - - Video Device - - - 139, 13 - - - 245, 21 - - - 30 - - - NoControl - - - 139, 117 - - - 99, 23 - - - 31 - - - Joystick Setup - - - NoControl - - - 471, 11 - - - 75, 23 - - - 32 - - - Stop - - - NoControl - - - 390, 11 - - - 75, 23 - - - 33 - - - Start - - - 4, 22 - - - 722, 434 - - - 3 - - - Setup - Bottom, Left diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml index a0a06996e2..4ba7a952cc 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/bin/Release/dataflashlog.xml @@ -22,9 +22,8 @@ WP Dist WP Verify Target Bear - Nav Bear - Long Err - Lat Err + Long Err + Lat Err Yaw Sensor @@ -56,6 +55,13 @@ Accel Y Accel Z + + Throttle in + Throttle intergrator + Voltage + Current + Current total + diff --git a/Tools/ArdupilotMegaPlanner/dataflashlog.xml b/Tools/ArdupilotMegaPlanner/dataflashlog.xml index a0a06996e2..4ba7a952cc 100644 --- a/Tools/ArdupilotMegaPlanner/dataflashlog.xml +++ b/Tools/ArdupilotMegaPlanner/dataflashlog.xml @@ -22,9 +22,8 @@ WP Dist WP Verify Target Bear - Nav Bear - Long Err - Lat Err + Long Err + Lat Err Yaw Sensor @@ -56,6 +55,13 @@ Accel Y Accel Z + + Throttle in + Throttle intergrator + Voltage + Current + Current total + diff --git a/Tools/ArdupilotMegaPlanner/georefimage.cs b/Tools/ArdupilotMegaPlanner/georefimage.cs new file mode 100644 index 0000000000..6ab16b82de --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/georefimage.cs @@ -0,0 +1,127 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Drawing; +using System.Drawing.Imaging; +using System.IO; + +namespace ArdupilotMega +{ + class georefimage + { + + public string logFile = ""; + public string dirWithImages = ""; + + DateTime getPhotoTime(string fn) + { + DateTime dtaken = DateTime.MinValue; + + try + { + Image myImage = Image.FromFile(fn); + PropertyItem propItem = myImage.GetPropertyItem(36867); // 36867 // 306 + + //Convert date taken metadata to a DateTime object + string sdate = Encoding.UTF8.GetString(propItem.Value).Trim(); + string secondhalf = sdate.Substring(sdate.IndexOf(" "), (sdate.Length - sdate.IndexOf(" "))); + string firsthalf = sdate.Substring(0, 10); + firsthalf = firsthalf.Replace(":", "-"); + sdate = firsthalf + secondhalf; + dtaken = DateTime.Parse(sdate); + + myImage.Dispose(); + } + catch { } + + return dtaken; + } + + List readLog(string fn) + { + List list = new List(); + + StreamReader sr = new StreamReader(fn); + + string lasttime = "0"; + + while (!sr.EndOfStream) + { + string line = sr.ReadLine(); + + if (line.ToLower().StartsWith("gps")) + { + string[] vals = line.Split(new char[] {',',':'}); + + if (lasttime == vals[1]) + continue; + + lasttime = vals[1]; + + list.Add(vals); + } + } + + sr.Close(); + sr.Dispose(); + + return list; + } + + public void dowork(float offsetseconds) + { + DateTime startTime = DateTime.MinValue; + + logFile = @"C:\Users\hog\Pictures\sams mums 22-6-2011\23-06-11 10-03 4.log"; + + List list = readLog(logFile); + + dirWithImages = @"C:\Users\hog\Pictures\sams mums 22-6-2011"; + + string[] files = Directory.GetFiles(dirWithImages); + + StreamWriter sw2 = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.txt"); + + StreamWriter sw = new StreamWriter(dirWithImages + Path.DirectorySeparatorChar + "location.tel"); + sw.WriteLine("version=1"); + sw.WriteLine("#longitude and latitude - in degrees"); + sw.WriteLine("#name utc longitude latitude height"); + + foreach (string file in files) + { + if (file.ToLower().EndsWith(".jpg")) + { + DateTime dt = getPhotoTime(file); + + if (startTime == DateTime.MinValue) + startTime = new DateTime(dt.Year,dt.Month,dt.Day,0,0,0,0,DateTimeKind.Utc).ToLocalTime(); + + foreach (string[] arr in list) + { + DateTime crap = startTime.AddMilliseconds(int.Parse(arr[1])).AddSeconds(offsetseconds); + + //Console.Write(dt + " " + crap + "\r"); + + if (dt.Equals(crap)) + { + sw2.WriteLine(Path.GetFileNameWithoutExtension(file) + " " + arr[5] + " " + arr[4] + " " + arr[6]); + sw.WriteLine(Path.GetFileNameWithoutExtension(file) + "\t" + crap.ToString("yyyy:MM:dd HH:mm:ss") +"\t"+ arr[5] + "\t" + arr[4] + "\t" + arr[6]); + sw.Flush(); + sw2.Flush(); + Console.WriteLine(Path.GetFileNameWithoutExtension(file) + " " + arr[5] + " " + arr[4] + " " + arr[6] + " "); + break; + } + //Console.WriteLine(crap); + } + } + + + } + + sw2.Close(); + sw.Close(); + + } + } +} diff --git a/Tools/ArdupilotMegaPlanner/hudold.cs b/Tools/ArdupilotMegaPlanner/hudold.cs new file mode 100644 index 0000000000..a65755d2a0 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/hudold.cs @@ -0,0 +1,1149 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Drawing; +using System.Data; +using System.Text; +using System.Windows.Forms; +using System.IO; +using System.Drawing.Imaging; + +using System.Threading; + + + +using System.Drawing.Drawing2D; + +using OpenTK; +using OpenTK.Graphics.OpenGL; + +// Control written by Michael Oborne 2011 + +namespace hud +{ + public partial class HUD : MyUserControl //GLControl + { + object paintlock = new object(); + object streamlock = new object(); + MemoryStream _streamjpg = new MemoryStream(); + public MemoryStream streamjpg { get { lock (streamlock) { return _streamjpg; } } set { lock (streamlock) { _streamjpg = value; } } } + /// + /// this is to reduce cpu usage + /// + public bool streamjpgenable = false; + + int huddrawtime = 0; + + public HUD() + { + InitializeComponent(); + //graphicsObject = this; + graphicsObject = Graphics.FromImage(objBitmap); + } + + private void InitializeComponent() + { + System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(HUD)); + this.SuspendLayout(); + // + // HUD + // + this.BackColor = System.Drawing.Color.Black; + this.Name = "HUD"; + resources.ApplyResources(this, "$this"); + this.ResumeLayout(false); + + } + + float _roll; + float _navroll; + float _pitch; + float _navpitch; + float _heading; + float _targetheading; + float _alt; + float _targetalt; + float _groundspeed; + float _airspeed; + float _targetspeed; + float _batterylevel; + float _batteryremaining; + float _gpsfix; + float _gpshdop; + float _disttowp; + float _groundcourse; + float _xtrack_error; + float _turnrate; + float _verticalspeed; + string _mode = "Manual"; + int _wpno; + + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float roll { get { return _roll; } set { if (_roll != value) { _roll = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float navroll { get { return _navroll; } set { if (_navroll != value) { _navroll = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float pitch { get { return _pitch; } set { if (_pitch != value) { _pitch = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float navpitch { get { return _navpitch; } set { if (_navpitch != value) { _navpitch = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float heading { get { return _heading; } set { if (_heading != value) { _heading = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float targetheading { get { return _targetheading; } set { if (_targetheading != value) { _targetheading = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float alt { get { return _alt; } set { if (_alt != value) { _alt = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float targetalt { get { return _targetalt; } set { if (_targetalt != value) { _targetalt = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float groundspeed { get { return _groundspeed; } set { if (_groundspeed != value) { _groundspeed = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float airspeed { get { return _airspeed; } set { if (_airspeed != value) { _airspeed = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float targetspeed { get { return _targetspeed; } set { if (_targetspeed != value) { _targetspeed = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float batterylevel { get { return _batterylevel; } set { if (_batterylevel != value) { _batterylevel = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float batteryremaining { get { return _batteryremaining; } set { if (_batteryremaining != value) { _batteryremaining = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float gpsfix { get { return _gpsfix; } set { if (_gpsfix != value) { _gpsfix = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float gpshdop { get { return _gpshdop; } set { if (_gpshdop != value) { _gpshdop = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float disttowp { get { return _disttowp; } set { if (_disttowp != value) { _disttowp = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public string mode { get { return _mode; } set { if (_mode != value) { _mode = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public int wpno { get { return _wpno; } set { if (_wpno != value) { _wpno = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float groundcourse { get { return _groundcourse; } set { if (_groundcourse != value) { _groundcourse = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float xtrack_error { get { return _xtrack_error; } set { if (_xtrack_error != value) { _xtrack_error = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float turnrate { get { return _turnrate; } set { if (_turnrate != value) { _turnrate = value; this.Invalidate(); } } } + [System.ComponentModel.Browsable(true), System.ComponentModel.Category("Values")] + public float verticalspeed { get { return _verticalspeed; } set { if (_verticalspeed != value) { _verticalspeed = value; this.Invalidate(); } } } + + public bool bgon = true; + public bool hudon = true; + + [System.ComponentModel.Browsable(true), +System.ComponentModel.Category("Values")] + public Color hudcolor { get { return whitePen.Color; } set { whitePen = new Pen(value, 2); } } + + Pen whitePen = new Pen(Color.White, 2); + + public Image bgimage { set { _bgimage = value; this.Invalidate(); } } + Image _bgimage; + + // move these global as they rarely change - reduce GC + Font font = new Font("Arial", 10); + Bitmap objBitmap = new Bitmap(640, 480); + int count = 0; + DateTime countdate = DateTime.Now; + Graphics graphicsObject; // Graphics + + DateTime starttime = DateTime.MinValue; + + System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(HUD)); + + public override void Refresh() + { + base.Refresh(); + OnPaint(new PaintEventArgs(this.CreateGraphics(),this.ClientRectangle)); + } + + protected override void OnLoad(EventArgs e) + { + base.OnLoad(e); + + /* + GL.MatrixMode(MatrixMode.Projection); + GL.LoadIdentity(); + GL.Ortho(0, Width, Height, 0, -1, 1); + GL.MatrixMode(MatrixMode.Modelview); + GL.LoadIdentity(); + + GL.Disable(EnableCap.DepthTest); + */ + //GL.Viewport(0, 0, Width, Height); + } + + protected override void OnPaint(PaintEventArgs e) + { + //GL.Enable(EnableCap.AlphaTest); + +// GL.ClearColor(Color.Red); + + // GL.Clear(ClearBufferMask.ColorBufferBit); + + //GL.LoadIdentity(); + +// GL.Viewport(0, 0, Width, Height); + + doPaint(e); + + //this.SwapBuffers(); + + //MakeCurrent(); + } + + void Clear(Color color) + { + GL.ClearColor(color); + } + + const float rad2deg = (float)(180 / Math.PI); + const float deg2rad = (float)(1.0 / rad2deg); + + //graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90); + + void DrawArc(Pen penn,RectangleF rect, float start,float degrees) + { + GL.Begin(BeginMode.Lines); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + start -= 90; + float x, y; + for (int i = (int)start; i <= start + degrees; i++) + { + x = (float)Math.Sin(i * deg2rad) * rect.Width / 2; + y = (float)Math.Cos(i * deg2rad) * rect.Height / 2; + x = x + rect.X + rect.Width / 2; + y = y + rect.Y + rect.Height / 2; + GL.Vertex2(x, y); + } + GL.End(); + } + + void DrawEllipse(Pen penn, Rectangle rect) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + float x, y; + for (float i = 0; i < 360; i+=1) + { + x = (float)Math.Sin(i * deg2rad) * rect.Width / 2; + y = (float)Math.Cos(i * deg2rad) * rect.Height / 2; + x = x + rect.X + rect.Width / 2; + y = y + rect.Y + rect.Height / 2; + GL.Vertex2(x, y); + } + GL.End(); + } + + //graphicsObject.DrawImage(_bgimage, 0, 0, this.Width, this.Height); + + void DrawImage(Image img, int x, int y, int width, int height) + { + Bitmap bitmap = (Bitmap)img; + int texture; + + GL.GenTextures(1, out texture); + GL.BindTexture(TextureTarget.Texture2D, texture); + + BitmapData data = bitmap.LockBits(new System.Drawing.Rectangle(0, 0, bitmap.Width, bitmap.Height), + ImageLockMode.ReadOnly, System.Drawing.Imaging.PixelFormat.Format32bppArgb); + + GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgba, data.Width, data.Height, 0, + OpenTK.Graphics.OpenGL.PixelFormat.Bgra, PixelType.UnsignedByte, data.Scan0); + + bitmap.UnlockBits(data); + + GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Linear); + GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Linear); + + + + GL.MatrixMode(MatrixMode.Modelview); + GL.LoadIdentity(); + GL.BindTexture(TextureTarget.Texture2D, texture); + + GL.Begin(BeginMode.Quads); + + GL.TexCoord2(0.0f, 1.0f); GL.Vertex2(-0.6f, -0.4f); + GL.TexCoord2(1.0f, 1.0f); GL.Vertex2(0.6f, -0.4f); + GL.TexCoord2(1.0f, 0.0f); GL.Vertex2(0.6f, 0.4f); + GL.TexCoord2(0.0f, 0.0f); GL.Vertex2(-0.6f, 0.4f); + + GL.End(); + } + + void DrawPath(Pen penn,GraphicsPath gp) + { + try + { + DrawPolygon(penn, gp.PathPoints); + } + catch { } + } + + void FillPath(Brush brushh,GraphicsPath gp) + { + try + { + FillPolygon(brushh, gp.PathPoints); + } + catch { } + } + + SmoothingMode SmoothingMode; + + void SetClip(Rectangle rect) + { + + } + + void ResetClip() + { + + } + + void ResetTransform() + { + GL.LoadIdentity(); + } + + void RotateTransform(float angle) + { + GL.Rotate(angle,0,0,1); + } + + void TranslateTransform(float x, float y) + { + GL.Translate(x, y, 0f); + } + + void FillPolygon(Brush brushh, Point[] list) + { + GL.Begin(BeginMode.TriangleFan); + GL.Color4(((SolidBrush)brushh).Color); + foreach (Point pnt in list) + { + GL.Vertex2(pnt.X, pnt.Y); + } + GL.Vertex2(list[list.Length - 1].X, list[list.Length - 1].Y); + GL.End(); + } + + void FillPolygon(Brush brushh, PointF[] list) + { + GL.Begin(BeginMode.Quads); + GL.Color4(((SolidBrush)brushh).Color); + foreach (PointF pnt in list) + { + GL.Vertex2(pnt.X, pnt.Y); + } + GL.Vertex2(list[0].X, list[0].Y); + GL.End(); + } + + //graphicsObject.DrawPolygon(redPen, pointlist); + + void DrawPolygon(Pen penn, Point[] list) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + foreach (Point pnt in list) + { + GL.Vertex2(pnt.X,pnt.Y); + } + GL.End(); + } + + void DrawPolygon(Pen penn, PointF[] list) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + foreach (PointF pnt in list) + { + GL.Vertex2(pnt.X, pnt.Y); + } + //GL.Vertex2(list[0].X, list[0].Y); + GL.End(); + } + + //graphicsObject.FillRectangle(linearBrush, bg); + + void FillRectangle(Brush brushh,RectangleF rectf) + { + float x1 = rectf.X; + float y1 = rectf.Y; + + float width = rectf.Width; + float height = rectf.Height; + + GL.Begin(BeginMode.Quads); + + if (((Type)brushh.GetType()) == typeof(LinearGradientBrush)) + { + LinearGradientBrush temp = (LinearGradientBrush)brushh; + GL.Color4(temp.LinearColors[0]); + } + else + { + GL.Color4(((SolidBrush)brushh).Color); + } + + GL.Vertex2(x1, y1); + GL.Vertex2(x1 + width, y1); + + if (((Type)brushh.GetType()) == typeof(LinearGradientBrush)) + { + LinearGradientBrush temp = (LinearGradientBrush)brushh; + GL.Color4(temp.LinearColors[1]); + } + else + { + GL.Color4(((SolidBrush)brushh).Color); + } + + GL.Vertex2(x1 + width, y1 + height); + GL.Vertex2(x1, y1 + height); + GL.End(); + } + + //graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height); + + void DrawRectangle(Pen penn, RectangleF rect) + { + DrawRectangle(penn, rect.X, rect.Y, rect.Width, rect.Height); + } + + void DrawRectangle(Pen penn,double x1,double y1, double width,double height) + { + GL.Begin(BeginMode.LineLoop); + GL.LineWidth(penn.Width); + GL.Color4(penn.Color); + GL.Vertex2(x1, y1); + GL.Vertex2(x1+width, y1); + GL.Vertex2(x1+width, y1+height); + GL.Vertex2(x1, y1+height); + GL.End(); + } + + void DrawLine(Pen penn,double x1,double y1, double x2,double y2) + { + GL.Begin(BeginMode.Lines); + GL.Color4(penn.Color); + GL.LineWidth(penn.Width); + GL.Vertex2(x1, y1); + GL.Vertex2(x2, y2); + GL.End(); + } + + void doPaint(object o) + { + PaintEventArgs e = (PaintEventArgs)o; + + try + { + // limit to 10hz ish + if ((DateTime.Now - starttime).TotalMilliseconds < 75 && (_bgimage == null)) + { + e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); + return; + } + + starttime = DateTime.Now; + + base.OnPaint(e); + + if (objBitmap.Width != this.Width || objBitmap.Height != this.Height) + { + objBitmap = new Bitmap(this.Width, this.Height); + graphicsObject = Graphics.FromImage(objBitmap); + + graphicsObject.SmoothingMode = SmoothingMode.AntiAlias; + graphicsObject.InterpolationMode = InterpolationMode.NearestNeighbor; + graphicsObject.CompositingMode = CompositingMode.SourceOver; + graphicsObject.CompositingQuality = CompositingQuality.HighSpeed; + graphicsObject.PixelOffsetMode = PixelOffsetMode.HighSpeed; + graphicsObject.TextRenderingHint = System.Drawing.Text.TextRenderingHint.SystemDefault; + } + + if (huddrawtime < 100) + { + graphicsObject.TextRenderingHint = System.Drawing.Text.TextRenderingHint.SystemDefault; + + graphicsObject.SmoothingMode = SmoothingMode.AntiAlias; + } + else + { + graphicsObject.SmoothingMode = SmoothingMode.HighSpeed; + } + + graphicsObject.Clear(Color.Gray); + + if (_bgimage != null) + { + bgon = false; + graphicsObject.DrawImage(_bgimage, 0, 0, this.Width, this.Height); + + if (hudon == false) + { + e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); + return; + } + } + else + { + bgon = true; + } + + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2); + + graphicsObject.RotateTransform(-roll); + + int fontsize = this.Height / 30; // = 10 + int fontoffset = fontsize - 10; + + float every5deg = -this.Height / 60; + + float pitchoffset = -pitch * every5deg; + + int halfwidth = this.Width / 2; + int halfheight = this.Height / 2; + + SolidBrush whiteBrush = new SolidBrush(whitePen.Color); + + Pen blackPen = new Pen(Color.Black, 2); + Pen greenPen = new Pen(Color.Green, 2); + Pen redPen = new Pen(Color.Red, 2); + + // draw sky + if (bgon == true) + { + RectangleF bg = new RectangleF(-halfwidth * 2, -halfheight * 2, this.Width * 2, halfheight * 2 + pitchoffset); + + if (bg.Height != 0) + { + LinearGradientBrush linearBrush = new LinearGradientBrush(bg, Color.Blue, + Color.LightBlue, LinearGradientMode.Vertical); + + Pen transPen = new Pen(Color.Transparent, 0); + + graphicsObject.DrawRectangle(transPen, bg.X,bg.Y,bg.Width,bg.Height); + + graphicsObject.FillRectangle(linearBrush, bg); + } + // draw ground + + bg = new RectangleF(-halfwidth * 2, pitchoffset, this.Width * 2, halfheight * 2 - pitchoffset); + + if (bg.Height != 0) + { + LinearGradientBrush linearBrush = new LinearGradientBrush(bg, Color.FromArgb(0x9b, 0xb8, 0x24), + Color.FromArgb(0x41, 0x4f, 0x07), LinearGradientMode.Vertical); + + Pen transPen = new Pen(Color.Transparent, 0); + + graphicsObject.DrawRectangle(transPen, bg.X, bg.Y, bg.Width, bg.Height); + + graphicsObject.FillRectangle(linearBrush, bg); + } + + //draw centerline + + graphicsObject.DrawLine(whitePen, -halfwidth * 2, pitchoffset + 0, halfwidth * 2, pitchoffset + 0); + } + + graphicsObject.ResetTransform(); + + graphicsObject.SetClip(new Rectangle(0, this.Height / 14, this.Width, this.Height - this.Height / 14)); + + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2); + graphicsObject.RotateTransform(-roll); + + //draw pitch + + int lengthshort = this.Width / 12; + int lengthlong = this.Width / 8; + + for (int a = -90; a <= 90; a += 5) + { + // limit to 40 degrees + if (a >= pitch - 34 && a <= pitch + 29) + { + if (a % 10 == 0) + { + if (a == 0) + { + graphicsObject.DrawLine(greenPen, this.Width / 2 - lengthlong - halfwidth, pitchoffset + a * every5deg, this.Width / 2 + lengthlong - halfwidth, pitchoffset + a * every5deg); + } + else + { + graphicsObject.DrawLine(whitePen, this.Width / 2 - lengthlong - halfwidth, pitchoffset + a * every5deg, this.Width / 2 + lengthlong - halfwidth, pitchoffset + a * every5deg); + } + drawstring(graphicsObject, a.ToString(), font, fontsize + 2, whiteBrush, this.Width / 2 - lengthlong - 30 - halfwidth - (int)(fontoffset * 1.7), pitchoffset + a * every5deg - 8 - fontoffset); + } + else + { + graphicsObject.DrawLine(whitePen, this.Width / 2 - lengthshort - halfwidth, pitchoffset + a * every5deg, this.Width / 2 + lengthshort - halfwidth, pitchoffset + a * every5deg); + //drawstring(e,a.ToString(), new Font("Arial", 10), whiteBrush, this.Width / 2 - lengthshort - 20 - halfwidth, this.Height / 2 + pitchoffset + a * every5deg - 8); + } + } + } + + graphicsObject.ResetTransform(); + + // draw roll ind needle + + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2 + this.Height / 14); + + graphicsObject.RotateTransform(-roll); + + Point[] pointlist = new Point[3]; + + lengthlong = this.Height / 66; + + int extra = this.Height / 15 * 7; + + pointlist[0] = new Point(0, -lengthlong * 2 - extra); + pointlist[1] = new Point(-lengthlong, -lengthlong - extra); + pointlist[2] = new Point(lengthlong, -lengthlong - extra); + + if (Math.Abs(roll) > 45) + { + redPen.Width = 10; + } + + graphicsObject.DrawPolygon(redPen, pointlist); + + redPen.Width = 2; + + for (int a = -45; a <= 45; a += 15) + { + graphicsObject.ResetTransform(); + graphicsObject.TranslateTransform(this.Width / 2, this.Height / 2 + this.Height / 14); + graphicsObject.RotateTransform(a); + drawstring(graphicsObject, Math.Abs(a).ToString("##"), font, fontsize, whiteBrush, 0 - 6 - fontoffset, -lengthlong * 2 - extra); + graphicsObject.DrawLine(whitePen, 0, -halfheight, 0, -halfheight - 10); + } + + graphicsObject.ResetTransform(); + + //draw centre / current att + + Rectangle centercircle = new Rectangle(halfwidth - 10, halfheight - 10, 20, 20); + + graphicsObject.DrawEllipse(redPen, centercircle); + graphicsObject.DrawLine(redPen, centercircle.Left - 10, halfheight, centercircle.Left, halfheight); + graphicsObject.DrawLine(redPen, centercircle.Right, halfheight, centercircle.Right + 10, halfheight); + graphicsObject.DrawLine(redPen, centercircle.Left + centercircle.Width / 2, centercircle.Top, centercircle.Left + centercircle.Width / 2, centercircle.Top - 10); + + // draw roll ind + + Rectangle arcrect = new Rectangle(this.Width / 2 - this.Height / 2, this.Height / 14, this.Height, this.Height); + + graphicsObject.DrawArc(whitePen, arcrect, 180 + 45, 90); + + //draw heading ind + + graphicsObject.ResetClip(); + + Rectangle headbg = new Rectangle(0, 0, this.Width - 0, this.Height / 14); + + graphicsObject.DrawRectangle(blackPen, headbg); + + SolidBrush solidBrush = new SolidBrush(Color.FromArgb(0x55, 0xff, 0xff, 0xff)); + + graphicsObject.FillRectangle(solidBrush, headbg); + + // center + graphicsObject.DrawLine(redPen, headbg.Width / 2, headbg.Bottom, headbg.Width / 2, headbg.Top); + + //bottom line + graphicsObject.DrawLine(whitePen, headbg.Left + 5, headbg.Bottom - 5, headbg.Width - 5, headbg.Bottom - 5); + + float space = (headbg.Width - 10) / 60.0f; + int start = ((int)heading - 30); + + // draw for outside the 60 deg + if (targetheading < start) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, headbg.Left + 5 + space * 0, headbg.Bottom, headbg.Left + 5 + space * (0), headbg.Top); + } + if (targetheading > heading + 30) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, headbg.Left + 5 + space * 60, headbg.Bottom, headbg.Left + 5 + space * (60), headbg.Top); + } + + for (int a = start; a <= heading + 30; a += 1) + { + // target heading + if (((a + 360) % 360) == targetheading) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, headbg.Left + 5 + space * (a - start), headbg.Bottom, headbg.Left + 5 + space * (a - start), headbg.Top); + } + + if (((a + 360) % 360) == (int)groundcourse) + { + blackPen.Width = 6; + graphicsObject.DrawLine(blackPen, headbg.Left + 5 + space * (a - start), headbg.Bottom, headbg.Left + 5 + space * (a - start), headbg.Top); + blackPen.Width = 2; + } + + if (a % 5 == 0) + { + //Console.WriteLine(space +" " + a +" "+ (headbg.Left + 5 + space * (a - start))); + graphicsObject.DrawLine(whitePen, headbg.Left + 5 + space * (a - start), headbg.Bottom - 5, headbg.Left + 5 + space * (a - start), headbg.Bottom - 10); + int disp = a; + if (disp < 0) + disp += 360; + disp = disp % 360; + if (disp == 0) + { + drawstring(graphicsObject, "N".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else if (disp == 90) + { + drawstring(graphicsObject, "E".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else if (disp == 180) + { + drawstring(graphicsObject, "S".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else if (disp == 270) + { + drawstring(graphicsObject, "W".PadLeft(2), font, fontsize + 4, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + else + { + drawstring(graphicsObject, (disp % 360).ToString().PadLeft(3), font, fontsize, whiteBrush, headbg.Left - 5 + space * (a - start) - fontoffset, headbg.Bottom - 24 - (int)(fontoffset * 1.7)); + } + } + } + + // Console.WriteLine("HUD 0 " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); + + // xtrack error + // center + + float xtspace = this.Width / 10.0f / 3.0f; + int pad = 10; + + float myxtrack_error = xtrack_error; + + myxtrack_error = Math.Min(myxtrack_error, 40); + myxtrack_error = Math.Max(myxtrack_error, -40); + + // xtrack - distance scale - space + float loc = myxtrack_error / 20.0f * xtspace; + + // current xtrack + if (Math.Abs(myxtrack_error) == 40) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + } + + graphicsObject.DrawLine(greenPen, this.Width / 10 + loc, headbg.Bottom + 5, this.Width / 10 + loc, headbg.Bottom + this.Height / 10); + + greenPen.Color = Color.FromArgb(255, greenPen.Color); + + graphicsObject.DrawLine(whitePen, this.Width / 10, headbg.Bottom + 5, this.Width / 10, headbg.Bottom + this.Height / 10); + + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace, headbg.Bottom + 5 + pad, this.Width / 10 - xtspace, headbg.Bottom + this.Height / 10 - pad); + + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace * 2, headbg.Bottom + 5 + pad, this.Width / 10 - xtspace * 2, headbg.Bottom + this.Height / 10 - pad); + + graphicsObject.DrawLine(whitePen, this.Width / 10 + xtspace, headbg.Bottom + 5 + pad, this.Width / 10 + xtspace, headbg.Bottom + this.Height / 10 - pad); + + graphicsObject.DrawLine(whitePen, this.Width / 10 + xtspace * 2, headbg.Bottom + 5 + pad, this.Width / 10 + xtspace * 2, headbg.Bottom + this.Height / 10 - pad); + + // rate of turn + + whitePen.Width = 4; + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace * 2 - xtspace / 2, headbg.Bottom + this.Height / 10 + 10, this.Width / 10 - xtspace * 2 - xtspace / 2 + xtspace, headbg.Bottom + this.Height / 10 + 10); + + graphicsObject.DrawLine(whitePen, this.Width / 10 - xtspace * 0 - xtspace / 2, headbg.Bottom + this.Height / 10 + 10, this.Width / 10 - xtspace * 0 - xtspace / 2 + xtspace, headbg.Bottom + this.Height / 10 + 10); + + graphicsObject.DrawLine(whitePen, this.Width / 10 + xtspace * 2 - xtspace / 2, headbg.Bottom + this.Height / 10 + 10, this.Width / 10 + xtspace * 2 - xtspace / 2 + xtspace, headbg.Bottom + this.Height / 10 + 10); + + float myturnrate = turnrate; + float trwidth = (this.Width / 10 + xtspace * 2 - xtspace / 2) - (this.Width / 10 - xtspace * 2 - xtspace / 2); + + float range = 12; + + myturnrate = Math.Min(myturnrate, range / 2); + myturnrate = Math.Max(myturnrate, (range / 2) * -1.0f); + + loc = myturnrate / range * trwidth; + + greenPen.Width = 4; + + if (Math.Abs(myturnrate) == (range / 2)) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + } + + graphicsObject.DrawLine(greenPen, this.Width / 10 + loc - xtspace / 2, headbg.Bottom + this.Height / 10 + 10 + 3, this.Width / 10 + loc + xtspace / 2, headbg.Bottom + this.Height / 10 + 10 + 3); + graphicsObject.DrawLine(greenPen, this.Width / 10 + loc, headbg.Bottom + this.Height / 10 + 10 + 3, this.Width / 10 + loc, headbg.Bottom + this.Height / 10 + 10 + 10); + + greenPen.Color = Color.FromArgb(255, greenPen.Color); + + whitePen.Width = 2; + + + + // left scroller + + Rectangle scrollbg = new Rectangle(0, halfheight - halfheight / 2, this.Width / 10, this.Height / 2); + + graphicsObject.DrawRectangle(whitePen, scrollbg); + + graphicsObject.FillRectangle(solidBrush, scrollbg); + + Point[] arrow = new Point[5]; + + arrow[0] = new Point(0, -10); + arrow[1] = new Point(scrollbg.Width - 10, -10); + arrow[2] = new Point(scrollbg.Width - 5, 0); + arrow[3] = new Point(scrollbg.Width - 10, 10); + arrow[4] = new Point(0, 10); + + graphicsObject.TranslateTransform(0, this.Height / 2); + + int viewrange = 26; + + float speed = airspeed; + if (speed == 0) + speed = groundspeed; + + space = (scrollbg.Height) / (float)viewrange; + start = ((int)speed - viewrange / 2); + + if (start > targetspeed) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top, scrollbg.Left + scrollbg.Width, scrollbg.Top); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + if ((speed + viewrange / 2) < targetspeed) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * viewrange, scrollbg.Left + scrollbg.Width, scrollbg.Top - space * viewrange); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + + for (int a = start; a <= (speed + viewrange / 2); a += 1) + { + if (a == (int)targetspeed && targetspeed != 0) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + scrollbg.Width, scrollbg.Top - space * (a - start)); + } + if (a % 5 == 0) + { + //Console.WriteLine(a + " " + scrollbg.Right + " " + (scrollbg.Top - space * (a - start)) + " " + (scrollbg.Right - 20) + " " + (scrollbg.Top - space * (a - start))); + graphicsObject.DrawLine(whitePen, scrollbg.Right, scrollbg.Top - space * (a - start), scrollbg.Right - 10, scrollbg.Top - space * (a - start)); + drawstring(graphicsObject, a.ToString().PadLeft(5), font, fontsize, whiteBrush, scrollbg.Right - 50 - 4 * fontoffset, scrollbg.Top - space * (a - start) - 6 - fontoffset); + } + } + + graphicsObject.DrawPolygon(blackPen, arrow); + graphicsObject.FillPolygon(Brushes.Black, arrow); + drawstring(graphicsObject, ((int)speed).ToString("0"), font, 10, Brushes.AliceBlue, 0, -9); + + graphicsObject.ResetTransform(); + + // extra text data + + drawstring(graphicsObject, "AS " + airspeed.ToString("0.0"), font, fontsize, whiteBrush, 1, scrollbg.Bottom + 5); + drawstring(graphicsObject, "GS " + groundspeed.ToString("0.0"), font, fontsize, whiteBrush, 1, scrollbg.Bottom + fontsize + 2 + 10); + + //drawstring(e,, new Font("Arial", fontsize + 2), whiteBrush, 1, scrollbg.Bottom + fontsize + 2 + 10); + + // right scroller + + scrollbg = new Rectangle(this.Width - this.Width / 10, halfheight - halfheight / 2, this.Width / 10, this.Height / 2); + + graphicsObject.DrawRectangle(whitePen, scrollbg); + + graphicsObject.FillRectangle(solidBrush, scrollbg); + + arrow = new Point[5]; + + arrow[0] = new Point(0, -10); + arrow[1] = new Point(scrollbg.Width - 10, -10); + arrow[2] = new Point(scrollbg.Width - 5, 0); + arrow[3] = new Point(scrollbg.Width - 10, 10); + arrow[4] = new Point(0, 10); + + + + graphicsObject.TranslateTransform(0, this.Height / 2); + + + + + viewrange = 26; + + space = (scrollbg.Height) / (float)viewrange; + start = ((int)alt - viewrange / 2); + + if (start > targetalt) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top, scrollbg.Left + scrollbg.Width, scrollbg.Top); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + if ((alt + viewrange / 2) < targetalt) + { + greenPen.Color = Color.FromArgb(128, greenPen.Color); + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * viewrange, scrollbg.Left + scrollbg.Width, scrollbg.Top - space * viewrange); + greenPen.Color = Color.FromArgb(255, greenPen.Color); + } + + for (int a = start; a <= (alt + viewrange / 2); a += 1) + { + if (a == Math.Round(targetalt) && targetalt != 0) + { + greenPen.Width = 6; + graphicsObject.DrawLine(greenPen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + scrollbg.Width, scrollbg.Top - space * (a - start)); + } + if (a % 5 == 0) + { + //Console.WriteLine(a + " " + scrollbg.Left + " " + (scrollbg.Top - space * (a - start)) + " " + (scrollbg.Left + 20) + " " + (scrollbg.Top - space * (a - start))); + graphicsObject.DrawLine(whitePen, scrollbg.Left, scrollbg.Top - space * (a - start), scrollbg.Left + 10, scrollbg.Top - space * (a - start)); + drawstring(graphicsObject, a.ToString().PadLeft(5), font, fontsize, whiteBrush, scrollbg.Left + 7 + (int)(0 * fontoffset), scrollbg.Top - space * (a - start) - 6 - fontoffset); + } + } + + + + // vsi + + graphicsObject.ResetTransform(); + + PointF[] poly = new PointF[4]; + + poly[0] = new PointF(scrollbg.Left, scrollbg.Top); + poly[1] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Top + scrollbg.Width / 4); + poly[2] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Bottom - scrollbg.Width / 4); + poly[3] = new PointF(scrollbg.Left, scrollbg.Bottom); + + //verticalspeed + + viewrange = 12; + + verticalspeed = Math.Min(viewrange / 2, verticalspeed); + verticalspeed = Math.Max(viewrange / -2, verticalspeed); + + float scaledvalue = verticalspeed / -viewrange * (scrollbg.Bottom - scrollbg.Top); + + float linespace = (float)1 / -viewrange * (scrollbg.Bottom - scrollbg.Top); + + PointF[] polyn = new PointF[4]; + + polyn[0] = new PointF(scrollbg.Left, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2); + polyn[1] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2); + polyn[2] = polyn[1]; + float peak = 0; + if (scaledvalue > 0) + { + peak = -scrollbg.Width / 4; + if (scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue + peak < scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2) + peak = -scaledvalue; + } + else if (scaledvalue < 0) + { + peak = +scrollbg.Width / 4; + if (scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue + peak > scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2) + peak = -scaledvalue; + } + + polyn[2] = new PointF(scrollbg.Left - scrollbg.Width / 4, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue + peak); + polyn[3] = new PointF(scrollbg.Left, scrollbg.Top + (scrollbg.Bottom - scrollbg.Top) / 2 + scaledvalue); + + //graphicsObject.DrawPolygon(redPen, poly); + graphicsObject.FillPolygon(Brushes.Blue, polyn); + + // draw outsidebox + graphicsObject.DrawPolygon(whitePen, poly); + + for (int a = 1; a < viewrange; a++) + { + graphicsObject.DrawLine(whitePen, scrollbg.Left - scrollbg.Width / 4, scrollbg.Top - linespace * a, scrollbg.Left - scrollbg.Width / 8, scrollbg.Top - linespace * a); + } + + // draw arrow and text + + graphicsObject.ResetTransform(); + graphicsObject.TranslateTransform(this.Width, this.Height / 2); + graphicsObject.RotateTransform(180); + + graphicsObject.DrawPolygon(blackPen, arrow); + graphicsObject.FillPolygon(Brushes.Black, arrow); + graphicsObject.ResetTransform(); + graphicsObject.TranslateTransform(0, this.Height / 2); + + drawstring(graphicsObject, ((int)alt).ToString("0"), font, 10, Brushes.AliceBlue, scrollbg.Left + 10, -9); + graphicsObject.ResetTransform(); + + // mode and wp dist and wp + + drawstring(graphicsObject, mode, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + 5); + drawstring(graphicsObject, (int)disttowp + ">" + wpno, font, fontsize, whiteBrush, scrollbg.Left - 30, scrollbg.Bottom + fontsize + 2 + 10); + + // battery + + graphicsObject.ResetTransform(); + + drawstring(graphicsObject, resources.GetString("Bat"), font, fontsize + 2, whiteBrush, fontsize, this.Height - 30 - fontoffset); + drawstring(graphicsObject, batterylevel.ToString("0.00v"), font, fontsize + 2, whiteBrush, fontsize * 4, this.Height - 30 - fontoffset); + drawstring(graphicsObject, batteryremaining.ToString("0%"), font, fontsize + 2, whiteBrush, fontsize * 9, this.Height - 30 - fontoffset); + + // gps + + string gps = ""; + + if (gpsfix == 0) + { + gps = resources.GetString("GPS: No Fix.Text"); + } + else if (gpsfix == 1) + { + gps = resources.GetString("GPS: No Fix.Text"); + } + else if (gpsfix == 2) + { + gps = resources.GetString("GPS: 2D Fix.Text"); + } + else if (gpsfix == 3) + { + gps = resources.GetString("GPS: 3D Fix.Text"); + } + + drawstring(graphicsObject, gps, font, fontsize + 2, whiteBrush, this.Width - 10 * fontsize, this.Height - 30 - fontoffset); + + e.Graphics.DrawImageUnscaled(objBitmap, 0, 0); + + if (DesignMode) + { + return; + } + + // Console.WriteLine("HUD 1 " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); + + ImageCodecInfo ici = GetImageCodec("image/jpeg"); + EncoderParameters eps = new EncoderParameters(1); + eps.Param[0] = new EncoderParameter(System.Drawing.Imaging.Encoder.Quality, 50L); // or whatever other quality value you want + + lock (streamlock) + { + if (streamjpgenable || streamjpg == null) // init image and only update when needed + { + streamjpg = new MemoryStream(); + objBitmap.Save(streamjpg, ici, eps); + //objBitmap.Save(streamjpg,ImageFormat.Bmp); + } + } + } + catch (Exception ex) + { + Console.WriteLine("hud error "+ex.ToString()); + //MessageBox.Show(ex.ToString()); + } + + count++; + + if (DateTime.Now.Second != countdate.Second) + { + countdate = DateTime.Now; + //Console.WriteLine("HUD " + count + " hz"); + count = 0; + } + huddrawtime = (int)(DateTime.Now - starttime).TotalMilliseconds; +#if DEBUG + // Console.WriteLine("HUD e " + (DateTime.Now - starttime).TotalMilliseconds + " " + DateTime.Now.Millisecond); +#endif + } + + protected override void OnPaintBackground(PaintEventArgs e) + { + //base.OnPaintBackground(e); + } + + ImageCodecInfo GetImageCodec(string mimetype) + { + foreach (ImageCodecInfo ici in ImageCodecInfo.GetImageEncoders()) + { + if (ici.MimeType == mimetype) return ici; + } + return null; + } + + + float wrap360(float noin) + { + if (noin < 0) + return noin + 360; + return noin; + } + + /// + /// pen for drawstring + /// + Pen P = new Pen(Color.FromArgb(0x26, 0x27, 0x28), 2f); + /// + /// pth for drawstring + /// + GraphicsPath pth = new GraphicsPath(); + + void drawstring(Graphics e, string text, Font font, float fontsize, Brush brush, float x, float y) + { + if (text == null || text == "") + return; + + pth.Reset(); + + + if (text != null) + pth.AddString(text, font.FontFamily, 0, fontsize + 5, new Point((int)x, (int)y), StringFormat.GenericTypographic); + + //Draw the edge + // this uses lots of cpu time + + //e.SmoothingMode = SmoothingMode.HighSpeed; + + e.DrawPath(P, pth); + + //Draw the face + + e.FillPath(brush, pth); + + //pth.Dispose(); + } + + protected override void OnResize(EventArgs e) + { + if (DesignMode) + return; + this.Height = (int)(this.Width / 1.333f); + base.OnResize(e); + /* + try + { + GL.MatrixMode(MatrixMode.Projection); + GL.LoadIdentity(); + GL.Ortho(0, Width, Height, 0, -1, 1); + GL.MatrixMode(MatrixMode.Modelview); + GL.LoadIdentity(); + + GL.Viewport(0, 0, Width, Height); + } + catch { } + */ + } + } +} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl b/Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl new file mode 100644 index 0000000000..b22c7dd9e7 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/mavlinklist-xml-dontuse.pl @@ -0,0 +1,132 @@ +$dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/message_definitions/"; +#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/pixhawk-mavlink-c91adfb/include/common/"; + +opendir(DIR,$dir) || die print $!; +@files = readdir(DIR); +closedir(DIR); + +open(OUT,">MAVLinkTypes.cs"); + +print OUT <) { + if ($line =~ /enum name="(MAV_.*)"/) { + $start = 1; + print OUT "\t\tpublic enum $1\n\t\t{ \n"; + } + + if ($line =~ //) { + $name = lc($2); + + print OUT "\t\tpublic const byte MAVLINK_MSG_ID_".uc($name) . " = " . $1 . ";\n"; + print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n"; + print OUT "\t\tpublic struct __mavlink_".$name."_t\n\t\t{\n"; + $no = $1; + + $start = 1; + + #__mavlink_gps_raw_t + $structs[$no] = "__mavlink_".$name."_t"; + } # __mavlink_heartbeat_t + + $line =~ s/MAV_CMD_NAV_//; + + $line =~ s/MAV_CMD_//; + + if ($line =~ //) + { + + + print OUT "\t\t\t$2 = $1,\n"; + + } + + # + if ($line =~ /(.*)<\/field>/) + { + + $type = $1; + $name = $2; + $desc = $3; + + print "$type = $name\n"; + + $type =~ s/byte_mavlink_version/public byte/; + + $type =~ s/array/public byte/; + + + + $type =~ s/uint8_t/public byte/; + $type =~ s/int8_t/public byte/; + $type =~ s/float/public float/; + $type =~ s/uint16_t/public ushort/; + $type =~ s/uint32_t/public uint/; + $type =~ s/uint64_t/public ulong/; + $type =~ s/int16_t/public short/; + $type =~ s/int32_t/public int/; + $type =~ s/int64_t/public long/; + + if ($type =~ /\[(.*)\]/) { # array + print OUT "\t\t\t[MarshalAs(UnmanagedType.ByValArray, SizeConst=". $1 .")] \n"; + $type =~ s/\[.*\]//; + $type =~ s/public\s+([^\s]+)/public $1\[\]/o; + } + + print OUT "\t\t\t$type $name; ///< $desc\n"; + + } + + if ($start && ($line =~ /<\/message>/ || $line =~ /<\/enum>/)) { + print OUT "\t\t};\n\n"; + $start = 0; + } + + } + + close(F); +} + +print OUT "Type[] mavstructs = new Type[] {"; +for ($a = 0; $a <= 256;$a++) +{ + if (defined($structs[$a])) { + print OUT "typeof(".$structs[$a] .") ,"; + } else { + print OUT "null ,"; + } +} +print OUT "};\n\n"; + +print OUT <MAVLinkTypes.cs"); +$crcs = 0; + print OUT <) { - if ($line =~ /enum name="(MAV_.*)"/) { - $start = 1; - print OUT "\t\tpublic enum $1\n\t\t{ \n"; + if ($line =~ /(MAVLINK_MESSAGE_LENGTHS|MAVLINK_MESSAGE_CRCS) (.*)/ && $crcs < 2) { + print OUT "\t\tpublic byte[] $1 = new byte[] $2;\n"; + $crcs++; } - if ($line =~ //) { - $name = lc($2); - - print OUT "\t\tpublic const byte MAVLINK_MSG_ID_".uc($name) . " = " . $1 . ";\n"; - print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n"; - print OUT "\t\tpublic struct __mavlink_".$name."_t\n\t\t{\n"; - $no = $1; - + if ($line =~ /enum (MAV_.*)/) { $start = 1; - + print OUT "\t\tpublic "; + } + + if ($line =~ /#define (MAVLINK_MSG_ID[^\s]+)\s+([0-9]+)/) { + print OUT "\t\tpublic const byte ".$1 . " = " . $2 . ";\n"; + $no = $2; + } + if ($line =~ /typedef struct(.*)/) { + if ($1 =~ /__mavlink_system|param_union/) { + last; + } + $start = 1; + print OUT "\t\t[StructLayout(LayoutKind.Sequential,Pack=1)]\n"; #__mavlink_gps_raw_t - $structs[$no] = "__mavlink_".$name."_t"; - } # __mavlink_heartbeat_t - - $line =~ s/MAV_CMD_NAV_//; - - $line =~ s/MAV_CMD_//; - - if ($line =~ //) - { - - - print OUT "\t\t\t$2 = $1,\n"; - + $structs[$no] = $1; } - - # - if ($line =~ /(.*)<\/field>/) - { - - $type = $1; - $name = $2; - $desc = $3; + if ($start) { + $line =~ s/MAV_CMD_NAV_//; - print "$type = $name\n"; + $line =~ s/MAV_CMD_//; - $type =~ s/byte_mavlink_version/public byte/; - - $type =~ s/array/public byte/; + $line =~ s/typedef/public/; + $line =~ s/uint8_t/public byte/; + $line =~ s/int8_t/public byte/; + $line =~ s/^\s+float/public float/; + $line =~ s/uint16_t/public ushort/; + $line =~ s/uint32_t/public uint/; + $line =~ s/uint64_t/public ulong/; + $line =~ s/int16_t/public short/; + $line =~ s/int32_t/public int/; + $line =~ s/int64_t/public long/; + $line =~ s/typedef/public/; + $line =~ s/}.*/};\n/; - - $type =~ s/uint8_t/public byte/; - $type =~ s/int8_t/public byte/; - $type =~ s/float/public float/; - $type =~ s/uint16_t/public ushort/; - $type =~ s/uint32_t/public uint/; - $type =~ s/uint64_t/public ulong/; - $type =~ s/int16_t/public short/; - $type =~ s/int32_t/public int/; - $type =~ s/int64_t/public long/; - - if ($type =~ /\[(.*)\]/) { # array - print OUT "\t\t\t[MarshalAs(UnmanagedType.ByValArray, SizeConst=". $1 .")] \n"; - $type =~ s/\[.*\]//; - $type =~ s/public\s+([^\s]+)/public $1\[\]/o; - } + if ($line =~ /\[(.*)\].*;/) { # array + print OUT "\t\t[MarshalAs( + UnmanagedType.ByValArray, + SizeConst=". $1 .")] \n"; + $line =~ s/\[.*\]//; + $line =~ s/public\s+([^\s]+)/public $1\[\]/o; + } - print OUT "\t\t\t$type $name; ///< $desc\n"; - - } - - if ($start && ($line =~ /<\/message>/ || $line =~ /<\/enum>/)) { - print OUT "\t\t};\n\n"; + print OUT "\t\t".$line; + } + if ($line =~ /}/) { $start = 0; } @@ -129,4 +128,6 @@ EOF close OUT; +; + 1; \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/srtm.cs b/Tools/ArdupilotMegaPlanner/srtm.cs index e11c6ecb9b..4b75ee5c52 100644 --- a/Tools/ArdupilotMegaPlanner/srtm.cs +++ b/Tools/ArdupilotMegaPlanner/srtm.cs @@ -14,7 +14,7 @@ namespace ArdupilotMega { short alt = -32768; - lat += 0.0008; + lat += 0.00083333333333333; //lng += 0.0008; int x = (int)Math.Floor(lng); @@ -32,44 +32,139 @@ namespace ArdupilotMega else ew = "W"; - string filename = ns+ Math.Abs(y).ToString("00")+ew+ Math.Abs(x).ToString("000")+".hgt"; + string filename = ns + Math.Abs(y).ToString("00") + ew + Math.Abs(x).ToString("000") + ".hgt"; + + string filename2 = "srtm_" + Math.Round((lng + 2.5 + 180) / 5, 0).ToString("00") + "_" + Math.Round((60 - lat + 2.5) / 5, 0).ToString("00") + ".asc"; + + if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) + { // srtm hgt files + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open, FileAccess.Read); + + float posx = 0; + float row = 0; + + if (fs.Length <= (1201 * 1201 * 2)) + { + posx = (int)(((float)(lng - x)) * (1201 * 2)); + row = (int)(((float)(lat - y)) * 1201) * (1201 * 2); + row = (1201 * 1201 * 2) - row; + } + else + { + posx = (int)(((float)(lng - x)) * (3601 * 2)); + row = (int)(((float)(lat - y)) * 3601) * (3601 * 2); + row = (3601 * 3601 * 2) - row; + } + + if (posx % 2 == 1) + { + posx--; + } + + //Console.WriteLine(filename + " row " + row + " posx" + posx); + + byte[] data = new byte[2]; + + fs.Seek((int)(row + posx), SeekOrigin.Begin); + fs.Read(data, 0, data.Length); + + fs.Close(); + fs.Dispose(); + + Array.Reverse(data); + + alt = BitConverter.ToInt16(data, 0); - if (!File.Exists(datadirectory + Path.DirectorySeparatorChar + filename)) - { return alt; } - - FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename, FileMode.Open,FileAccess.Read); - - float posx = 0; - float row = 0; - - if (fs.Length <= (1201 * 1201 * 2)) { - posx = (int)(((float)(lng - x)) * (1201 * 2)); - row = (int)(((float)(lat - y)) * 1201) * (1201 * 2); - row = (1201 * 1201 * 2) - row; - } else { - posx = (int)(((float)(lng - x)) * (3601 * 2)); - row = (int)(((float)(lat - y)) * 3601) * (3601 * 2); - row = (3601 * 3601 * 2) - row; - } - - if (posx % 2 == 1) + else if (File.Exists(datadirectory + Path.DirectorySeparatorChar + filename2)) { - posx--; + // this is way to slow - and cacheing it will chew memory 6001 * 6001 * 4 = 144048004 bytes + FileStream fs = new FileStream(datadirectory + Path.DirectorySeparatorChar + filename2, FileMode.Open, FileAccess.Read); + + StreamReader sr = new StreamReader(fs); + + int nox = 0; + int noy = 0; + float left = 0; + float top = 0; + int nodata = -9999; + float cellsize = 0; + + int rowcounter = 0; + + float wantrow = 0; + float wantcol = 0; + + + while (!sr.EndOfStream) + { + string line = sr.ReadLine(); + + if (line.StartsWith("ncols")) + { + nox = int.Parse(line.Substring(line.IndexOf(' '))); + + //hgtdata = new int[nox * noy]; + } + else if (line.StartsWith("nrows")) + { + noy = int.Parse(line.Substring(line.IndexOf(' '))); + + //hgtdata = new int[nox * noy]; + } + else if (line.StartsWith("xllcorner")) + { + left = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("yllcorner")) + { + top = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("cellsize")) + { + cellsize = float.Parse(line.Substring(line.IndexOf(' '))); + } + else if (line.StartsWith("NODATA_value")) + { + nodata = int.Parse(line.Substring(line.IndexOf(' '))); + } + else + { + string[] data = line.Split(new char[] { ' ' }); + + if (data.Length == (nox + 1)) + { + + + + wantcol = (float)((lng - Math.Round(left,0))); + + wantrow = (float)((lat - Math.Round(top,0))); + + wantrow =(int)( wantrow / cellsize); + wantcol =(int)( wantcol / cellsize); + + wantrow = noy - wantrow; + + if (rowcounter == wantrow) + { + Console.WriteLine("{0} {1} {2} {3} ans {4} x {5}", lng, lat, left, top, data[(int)wantcol], (nox + wantcol * cellsize)); + + return int.Parse(data[(int)wantcol]); + } + + rowcounter++; + } + } + + + + } + + return alt; } - //Console.WriteLine(filename + " row " + row + " posx" + posx); - - byte[] data = new byte[2]; - - fs.Seek((int)(row + posx), SeekOrigin.Begin); - fs.Read(data, 0, data.Length); - - Array.Reverse(data); - - alt = BitConverter.ToInt16(data,0); - return alt; } } From 9d79e68e88f4caef34ddcb5423282f51697405fe Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 17 Sep 2011 23:49:32 +0800 Subject: [PATCH 091/100] APM Planner 1.0.70 fix's --- Tools/ArdupilotMegaPlanner/CurrentState.cs | 34 ++++++++++++++----- Tools/ArdupilotMegaPlanner/HUD.cs | 2 +- Tools/ArdupilotMegaPlanner/MAVLink.cs | 21 +++++++----- Tools/ArdupilotMegaPlanner/MainV2.cs | 24 ++++++++++++- .../Properties/AssemblyInfo.cs | 2 +- 5 files changed, 63 insertions(+), 20 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 8cf6af287d..1584a56b18 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -285,24 +285,21 @@ namespace ArdupilotMega mode = "Acro"; break; case (byte)102: - mode = "Simple"; - break; - case (byte)103: mode = "Alt Hold"; break; - case (byte)104: + case (byte)103: mode = "Auto"; break; - case (byte)105: + case (byte)104: mode = "Guided"; break; - case (byte)106: + case (byte)105: mode = "Loiter"; break; - case (byte)107: + case (byte)106: mode = "RTL"; break; - case (byte)108: + case (byte)107: mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL: @@ -546,6 +543,27 @@ namespace ArdupilotMega //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; } + if (MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null) + { + var imu = new MAVLink.__mavlink_scaled_imu_t(); + + object temp = imu; + + MAVLink.ByteArrayToStructure(MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6); + + imu = (MAVLink.__mavlink_scaled_imu_t)(temp); + + gx = imu.xgyro; + gy = imu.ygyro; + gz = imu.zgyro; + + ax = imu.xacc; + ay = imu.yacc; + az = imu.zacc; + + //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; + } + if (MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null) { MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t(); diff --git a/Tools/ArdupilotMegaPlanner/HUD.cs b/Tools/ArdupilotMegaPlanner/HUD.cs index 7a993861c8..a8746a1e1b 100644 --- a/Tools/ArdupilotMegaPlanner/HUD.cs +++ b/Tools/ArdupilotMegaPlanner/HUD.cs @@ -1066,7 +1066,7 @@ System.ComponentModel.Category("Values")] } } - + greenPen.Width = 4; // vsi diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 1f00bcefb7..dc503e48e2 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -210,6 +210,7 @@ namespace ArdupilotMega Console.WriteLine("Done open " + sysid + " " + compid); + packetslost = 0; } public static byte[] StructureToByteArrayEndian(params object[] list) @@ -1491,6 +1492,7 @@ namespace ArdupilotMega } else { + MainV2.cs.datetime = DateTime.Now; temp[count] = (byte)BaseStream.ReadByte(); } } @@ -1565,6 +1567,15 @@ namespace ArdupilotMega Array.Resize(ref temp, count); + if (packetlosttimer.AddSeconds(10) < DateTime.Now) + { + packetlosttimer = DateTime.Now; + packetslost = (int)(packetslost * 0.8f); + packetsnotlost = (int)(packetsnotlost * 0.8f); + } + + MainV2.cs.linkqualitygcs = (ushort)((packetsnotlost / (packetsnotlost + packetslost)) * 100); + if (bpstime.Second != DateTime.Now.Second && !logreadmode) { Console.WriteLine("bps {0} loss {1} left {2}", bps1, synclost, BaseStream.BytesToRead); @@ -1602,6 +1613,7 @@ namespace ArdupilotMega try { + if ((temp[0] == 'U' || temp[0] == 254) && temp.Length >= temp[1]) { if (temp[2] != ((recvpacketcount + 1) % 0x100)) @@ -1620,15 +1632,6 @@ namespace ArdupilotMega Console.WriteLine("lost {0} pkts {1}", temp[2], (int)packetslost); } - if (packetlosttimer.AddSeconds(10) < DateTime.Now) - { - packetlosttimer = DateTime.Now; - packetslost = (int)(packetslost *0.8f); - packetsnotlost = (int)(packetsnotlost *0.8f); - } - - MainV2.cs.linkqualitygcs = (ushort)((packetsnotlost / (packetsnotlost + packetslost)) * 100); - packetsnotlost++; recvpacketcount = temp[2]; diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 8d2d26a0e7..00e90ad6e8 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -884,6 +884,8 @@ namespace ArdupilotMega DateTime speechcustomtime = DateTime.Now; + DateTime linkqualitytime = DateTime.Now; + while (serialthread) { try @@ -936,6 +938,21 @@ namespace ArdupilotMega speechcustomtime = DateTime.Now; } + if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10) + { + MainV2.cs.linkqualitygcs = 0; + } + + if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds >= 1) + { + GCSViews.FlightData.myhud.Invalidate(); + if (linkqualitytime.Second != DateTime.Now.Second) + { + MainV2.cs.linkqualitygcs = (ushort)(MainV2.cs.linkqualitygcs * 0.8f); + linkqualitytime = DateTime.Now; + } + } + if (speechenable && talk != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { float warnalt = float.MaxValue; @@ -975,7 +992,6 @@ namespace ArdupilotMega if (MainV2.talk.State == SynthesizerState.Ready) MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds"); } - MainV2.cs.linkqualitygcs = 0; } //Console.WriteLine(comPort.BaseStream.BytesToRead); @@ -1479,6 +1495,12 @@ namespace ArdupilotMega MainV2.comPort.Open(false); return true; } + if (keyData == (Keys.Control | Keys.Y)) // for ryan beall + { + MainV2.comPort.doAction(MAVLink.MAV_ACTION.MAV_ACTION_STORAGE_WRITE); + MessageBox.Show("Done MAV_ACTION_STORAGE_WRITE"); + return true; + } if (keyData == (Keys.Control | Keys.J)) // for jani { string data = "!!"; diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 2ee1a13020..622fe43525 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.69")] +[assembly: AssemblyFileVersion("1.0.70")] [assembly: NeutralResourcesLanguageAttribute("")] From b10493e205567fc89acfdbb9d31c6a9bd8dd3cec Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 17 Sep 2011 11:16:13 -0700 Subject: [PATCH 092/100] Reverted to current_loc.alt in MSG_VFR_HUD and MSG_Location --- ArduCopter/Mavlink_Common.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index a04ffecd4c..6ecb79761f 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -126,8 +126,8 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ chan, current_loc.lat, current_loc.lng, - /*current_loc.alt * 10,*/ // changed to absolute altitude - g_gps->altitude, + current_loc.alt * 10, // reverted to relative altitude + /*g_gps->altitude,*/ g_gps->ground_speed * rot.a.x, g_gps->ground_speed * rot.b.x, g_gps->ground_speed * rot.c.x); @@ -248,8 +248,8 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ (float)g_gps->ground_speed / 100.0, (dcm.yaw_sensor / 100) % 360, g.rc_3.servo_out/10, - /*current_loc.alt / 100.0,*/ // changed to absolute altitude - g_gps->altitude/100.0, + current_loc.alt / 100.0, + /*g_gps->altitude/100.0,*/ // reverted to relative altitude climb_rate); break; } From c9d9ee0d3b169c06d4c3901d748b7a0d65797b7b Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 17 Sep 2011 20:25:31 +0200 Subject: [PATCH 093/100] Merge some small misc improvements from APM_Camera branch --- .gitignore | 1 + ArduCopter/APM_Config.h | 4 ++- ArduCopter/ArduCopter.pde | 1 + ArduCopter/CMakeLists.txt | 1 - ArduCopter/defines.h | 7 ----- ArduCopter/motors.pde | 4 +-- ArduCopter/system.pde | 10 +++++--- ArduPlane/CMakeLists.txt | 34 +++++++++++++------------ ArduPlane/system.pde | 2 +- libraries/APM_RC/APM_RC.h | 2 ++ libraries/RC_Channel/RC_Channel_aux.cpp | 1 + 11 files changed, 35 insertions(+), 32 deletions(-) diff --git a/.gitignore b/.gitignore index 1d314f6281..88e0877b61 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .metadata/ Tools/ArdupilotMegaPlanner/bin/Release/logs/ config.mk +ArduCopter/Debug/ diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 870a07cc40..023e0c42fe 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -1,4 +1,6 @@ -// Example config file. Take a look at confi.h. Any term define there can be overridden by defining it here. +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// Example config file. Take a look at config.h. Any term define there can be overridden by defining it here. // GPS is auto-selected diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e715de63f7..9ddefbc965 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,5 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#define THISFIRMWARE "ArduCopter V2.0.43 Beta" /* ArduCopter Version 2.0 Beta Authors: Jason Short diff --git a/ArduCopter/CMakeLists.txt b/ArduCopter/CMakeLists.txt index bef981f926..4e282df19b 100644 --- a/ArduCopter/CMakeLists.txt +++ b/ArduCopter/CMakeLists.txt @@ -53,7 +53,6 @@ set(${FIRMWARE_NAME}_SKETCHES commands_process.pde control_modes.pde events.pde - flight_control.pde flip.pde GCS.pde GCS_Ardupilot.pde diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index f10c432b2a..7f1c1dcf62 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -83,13 +83,6 @@ #define MAX_SONAR_UNKNOWN 0 #define MAX_SONAR_XL 1 -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_10 9 //PB5 -#define CH_11 10 //PE3 - #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 5a4d0e3e9a..1a2b4689f2 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define ARM_DELAY 10 // one secon -#define DISARM_DELAY 10 // one secon +#define ARM_DELAY 10 // one second +#define DISARM_DELAY 10 // one second #define LEVEL_DELAY 120 // twelve seconds #define AUTO_LEVEL_DELAY 150 // twentyfive seconds diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 34d9acd1c4..1161b58d7e 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.43 Beta", main_menu_commands); +MENU(main_menu, THISFIRMWARE, main_menu_commands); #endif // CLI_ENABLED @@ -73,8 +73,8 @@ static void init_ardupilot() Serial1.begin(38400, 128, 16); #endif - Serial.printf_P(PSTR("\n\nInit ACM" - "\n\nRAM: %lu\n"), + Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE + "\n\nFree RAM: %lu\n"), freeRAM()); @@ -305,7 +305,9 @@ static void startup_ground(void) // Warm up and read Gyro offsets // ----------------------------- imu.init_gyro(mavlink_delay); - report_imu(); + #if CLI_ENABLED == ENABLED + report_imu(); + #endif #endif // reset the leds diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt index 1509de51a6..d2c7506d88 100644 --- a/ArduPlane/CMakeLists.txt +++ b/ArduPlane/CMakeLists.txt @@ -11,6 +11,9 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # A set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + +message(STATUS "DIR: ${CMAKE_SOURCE_DIR}") + cmake_minimum_required(VERSION 2.8) #====================================================================# # Setup Project # @@ -50,7 +53,6 @@ set(${FIRMWARE_NAME}_SKETCHES commands_process.pde control_modes.pde events.pde - #flight_control.pde #flip.pde #GCS.pde #GCS_Ardupilot.pde @@ -103,26 +105,26 @@ set(${FIRMWARE_NAME}_LIBS DataFlash AP_Math PID - RC_Channel + RC_Channel AP_OpticalFlow ModeFilter - memcheck + memcheck #AP_PID APM_PI #APO - FastSerial - AP_Common - GCS_MAVLink - AP_GPS - APM_RC - AP_DCM - AP_ADC - AP_Compass - AP_IMU - AP_RangeFinder - APM_BMP085 - c - m + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + c + m ) SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index cbe7ddadd2..4c994cdb3e 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -41,7 +41,7 @@ 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, THISFIRMWARE, main_menu_commands); #endif // CLI_ENABLED diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index bcac692abe..1c2064562e 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -15,6 +15,8 @@ #define CH_6 5 #define CH_7 6 #define CH_8 7 +#define CH_10 9 //PB5 +#define CH_11 10 //PE3 #include diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 1ff7b45326..3a2f1e6a7d 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -48,6 +48,7 @@ void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Ch g_rc_function[aux_servo_function[CH_7]] = rc_7; g_rc_function[aux_servo_function[CH_8]] = rc_8; + //set auxiliary ranges 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); From e81a666478da20b5d4ff7684e5bc1b4cc3ef02a1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 17 Sep 2011 12:23:16 -0700 Subject: [PATCH 094/100] Added a GPS watchdog to stop navigating if we loose signal. --- ArduCopter/ArduCopter.pde | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e715de63f7..a357d02db8 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -481,6 +481,7 @@ static long perf_mon_timer; //static float imu_health; // Metric based on accel gain deweighting static int G_Dt_max; // Max main loop cycle time in milliseconds static int gps_fix_count; +static byte gps_watchdog; // System Timers // -------------- @@ -936,8 +937,16 @@ static void update_GPS(void) //current_loc.lat = -1224318000; // Lat * 10 * *7 //current_loc.alt = 100; // alt * 10 * *7 //return; + if(gps_watchdog < 10){ + gps_watchdog++; + }else{ + // we have lost GPS signal for a moment. Reduce our error to avoid flyaways + nav_roll >>= 1; + nav_pitch >>= 1; + } if (g_gps->new_data && g_gps->fix) { + gps_watchdog = 0; // XXX We should be sending GPS data off one of the regular loops so that we send // no-GPS-fix data too From 95dd8cc35b4e07fe03178ace642fe2951f219dde Mon Sep 17 00:00:00 2001 From: Janne M Date: Tue, 13 Sep 2011 23:00:28 +0300 Subject: [PATCH 095/100] Added voltage divider, input voltage and amps per volt to parameters. --- ArduPlane/Parameters.h | 37 +++++++++++++++++++++++-------------- ArduPlane/defines.h | 8 ++++---- 2 files changed, 27 insertions(+), 18 deletions(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 70976ce887..a4c402ead6 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -18,10 +18,10 @@ public: // by newer code. // 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) - // GCS will interpret values 0-9 as ArduPilotMega. Developers may use + // 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 @@ -70,7 +70,7 @@ public: k_param_flap_2_speed, k_param_num_resets, - + // 110: Telemetry control // k_param_streamrates_port0 = 110, @@ -95,10 +95,13 @@ public: 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, + k_param_volt_div_ratio, + k_param_curr_amp_per_volt, + k_param_input_voltage, + k_param_pack_capacity, + k_param_airspeed_offset, + k_param_sonar_enabled, + k_param_airspeed_enabled, // // 150: Navigation parameters @@ -129,7 +132,7 @@ public: 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, @@ -222,13 +225,13 @@ public: 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; @@ -268,7 +271,7 @@ public: 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; @@ -308,6 +311,9 @@ public: 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_Float volt_div_ratio; + AP_Float curr_amp_per_volt; + AP_Float input_voltage; 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; @@ -377,7 +383,7 @@ public: 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")), @@ -412,9 +418,12 @@ public: 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")), + volt_div_ratio (VOLT_DIV_RATIO, k_param_volt_div_ratio, PSTR("VOLT_DIVIDER")), + curr_amp_per_volt (CURR_AMP_PER_VOLT, k_param_curr_amp_per_volt, PSTR("AMP_PER_VOLT")), + input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("INPUT_VOLTS")), 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")), diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 5c5b01b10f..1f638da505 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -72,7 +72,7 @@ #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 and Fly By Wire C require airspeed sensor #define AUTO 10 #define RTL 11 #define LOITER 12 @@ -105,7 +105,7 @@ /// 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 +/// stream #define MSG_ACKNOWLEDGE 0x00 #define MSG_HEARTBEAT 0x01 #define MSG_ATTITUDE 0x02 @@ -220,9 +220,9 @@ #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 BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio -#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT +#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt #define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor #define BATTERY_PIN1 0 // These are the pins for the voltage dividers From e433f727d8a377a4b271b1e4be69bfa2221e3ede Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 09:31:50 +1000 Subject: [PATCH 096/100] script to generate frame sizes from .lst files this is used by http://apm.tridgell.net --- Tools/scripts/frame_sizes.py | 44 ++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100755 Tools/scripts/frame_sizes.py diff --git a/Tools/scripts/frame_sizes.py b/Tools/scripts/frame_sizes.py new file mode 100755 index 0000000000..bf1e62c9c0 --- /dev/null +++ b/Tools/scripts/frame_sizes.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +import re, sys, operator, os + +code_line = re.compile("^\s*\d+:/") +frame_line = re.compile("^\s*\d+\s+/\* frame size = (\d+) \*/") + +class frame(object): + def __init__(self, code, frame_size): + self.code = code + self.frame_size = int(frame_size) + +frames = [] + +def process_lst(filename): + '''process one lst file''' + last_code = '' + h = open(filename, mode='r') + for line in h: + if code_line.match(line): + last_code = line.strip() + elif frame_line.match(line): + frames.append(frame(last_code, frame_line.match(line).group(1))) + h.close() + +if len(sys.argv) > 1: + dname = sys.argv[1] +else: + dname = '.' + +for root, dirs, files in os.walk(dname): + for f in files: + if f.endswith(".lst"): + process_lst(os.path.join(root, f)) + +sorted_frames = sorted(frames, + key=operator.attrgetter('frame_size'), + reverse=True) + +print("FrameSize Code") +for frame in sorted_frames: + if frame.frame_size > 0: + print("%9u %s" % (frame.frame_size, frame.code)) + From e40fe2293b507a68d682d64edd2da85daae238f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 11:03:27 +1000 Subject: [PATCH 097/100] MAVLink: enable separate MAVLink helpers this makes the core MAVLink functions into library calls, instead of being inlined. The resulting code size doesn't change, but it means we can safely call MAVLink functions from multiple places without causing undue code bloat --- libraries/GCS_MAVLink/GCS_MAVLink.cpp | 2 ++ libraries/GCS_MAVLink/GCS_MAVLink.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index 2cecade369..0ecc270f5a 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -10,3 +10,5 @@ BetterStream *mavlink_comm_1_port; // this might need to move to the flight software mavlink_system_t mavlink_system = {7,1,0,0}; + +#include "include/mavlink_helpers.h" diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index d89f3ee34d..5de542bc4a 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -8,6 +8,8 @@ #include +#define MAVLINK_SEPARATE_HELPERS + #include "include/ardupilotmega/version.h" // this allows us to make mavlink_message_t much smaller From ee1541cda77ee9d49fc31211feee14b11731e17e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 11:05:21 +1000 Subject: [PATCH 098/100] rework the MAVLink send code to avoid excessive stack usage this avoids a varient of the gcc excessive stack usage bug, by wrapping the send calls in NOINLINE functions. This saves us a lot of stack space, and strangely enough produces slightly smaller code! --- ArduPlane/Mavlink_Common.h | 675 ++++++++++++++++++++----------------- 1 file changed, 368 insertions(+), 307 deletions(-) diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..9923ab8f98 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -25,327 +25,388 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +#define NOINLINE __attribute__((noinline)) + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + Vector3f omega = dcm.get_gyro(); + mavlink_msg_attitude_send( + chan, + micros(), + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); +} + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case MANUAL: + mode = MAV_MODE_MANUAL; + break; + case STABILIZE: + mode = MAV_MODE_TEST1; + 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; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case LOITER: + 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; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + load * 1000, + battery_voltage * 1000, + battery_remaining, + packet_drops); +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + nav_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + airspeed_error, + crosstrack_error); +} + +static void NOINLINE send_local_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_local_position_send( + chan, + micros(), + ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, + ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), + (current_loc.alt - home.alt) / 1.0e2, + g_gps->ground_speed / 1.0e2 * rot.a.x, + g_gps->ground_speed / 1.0e2 * rot.b.x, + g_gps->ground_speed / 1.0e2 * rot.c.x); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + mavlink_msg_rc_channels_scaled_send( + chan, + 10000 * g.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + 0, + 0, + 0, + 0, + rssi); +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.channel_roll.radio_in, + g.channel_pitch.radio_in, + g.channel_throttle.radio_in, + g.channel_rudder.radio_in, + g.rc_5.radio_in, // XXX currently only 4 RC channels defined + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + g.channel_roll.radio_out, + g.channel_pitch.radio_out, + g.channel_throttle.radio_out, + g.channel_rudder.radio_out, + g.rc_5.radio_out, // XXX currently only 4 RC channels defined + g.rc_6.radio_out, + g.rc_7.radio_out, + g.rc_8.radio_out); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + (int)g.channel_throttle.servo_out, + current_loc.alt / 100.0, + climb_rate); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-g.ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + 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()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); +} + + // 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) { + switch (id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + return true; - case MSG_HEARTBEAT: - { - CHECK_PAYLOAD_SIZE(HEARTBEAT); - mavlink_msg_heartbeat_send( - chan, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; + + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; + + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; + + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; + + case MSG_NAV_CONTROLLER_OUTPUT: + if (control_mode != MANUAL) { + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); } + break; + + case MSG_LOCAL_LOCATION: + CHECK_PAYLOAD_SIZE(LOCAL_POSITION); + send_local_location(chan); + break; + + case MSG_GPS_RAW: + CHECK_PAYLOAD_SIZE(GPS_RAW); + send_gps_raw(chan); + break; + + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; + + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; + + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; + + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; + +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; + + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; + + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE + + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; + + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; - case MSG_EXTENDED_STATUS1: - { - CHECK_PAYLOAD_SIZE(SYS_STATUS); - - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; - - switch(control_mode) { - case MANUAL: - mode = MAV_MODE_MANUAL; - break; - case STABILIZE: - mode = MAV_MODE_TEST1; - 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; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case LOITER: - 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; - uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 - - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - load * 1000, - 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, - timeStamp, - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); - break; - } - - 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, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, - g_gps->ground_speed * rot.a.x, - g_gps->ground_speed * rot.b.x, - g_gps->ground_speed * rot.c.x); - break; - } - - 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, - nav_pitch / 1.0e2, - nav_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - airspeed_error, - crosstrack_error); - } - break; - } - - 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, - timeStamp, - ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, - ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), - (current_loc.alt - home.alt) / 1.0e2, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); - break; - } - - case MSG_GPS_RAW: - { - CHECK_PAYLOAD_SIZE(GPS_RAW); - mavlink_msg_gps_raw_send( - chan, - timeStamp, - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); - break; - } - - 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 - mavlink_msg_rc_channels_scaled_send( - chan, - 10000 * g.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.norm_output(), - 0, - 0, - 0, - 0, - rssi); - break; - } - - case MSG_RADIO_IN: - { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - uint8_t rssi = 1; - mavlink_msg_rc_channels_raw_send( - chan, - g.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, // XXX currently only 4 RC channels defined - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); - break; - } - - case MSG_RADIO_OUT: - { - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - mavlink_msg_servo_output_raw_send( - chan, - g.channel_roll.radio_out, - g.channel_pitch.radio_out, - g.channel_throttle.radio_out, - g.channel_rudder.radio_out, - g.rc_5.radio_out, // XXX currently only 4 RC channels defined - g.rc_6.radio_out, - g.rc_7.radio_out, - g.rc_8.radio_out); - break; - } - - case MSG_VFR_HUD: - { - CHECK_PAYLOAD_SIZE(VFR_HUD); - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, - (int)g.channel_throttle.servo_out, - current_loc.alt / 100.0, - climb_rate); - break; - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - 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( - chan, - timeStamp, - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); - break; - } - - case MSG_RAW_IMU2: - { - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - 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()); - 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, - NULL, - NULL, - NULL, - NULL, - NULL); - break; - } - - case MSG_CURRENT_WAYPOINT: - { - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); - mavlink_msg_waypoint_current_send( - chan, - g.waypoint_index); - break; - } - - default: - break; + default: + break; } return true; } From 46d6974da2c18e4b10cc3485f44f689728bc9a63 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 11:07:24 +1000 Subject: [PATCH 099/100] remove unsued MSG_LOCAL_LOCATION this saves us a bit of code --- ArduPlane/HIL_Xplane.pde | 2 -- ArduPlane/Mavlink_Common.h | 19 ------------------- ArduPlane/defines.h | 1 - 3 files changed, 22 deletions(-) diff --git a/ArduPlane/HIL_Xplane.pde b/ArduPlane/HIL_Xplane.pde index 4f01c19028..d87b01c344 100644 --- a/ArduPlane/HIL_Xplane.pde +++ b/ArduPlane/HIL_Xplane.pde @@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param) break; case MSG_LOCATION: break; - case MSG_LOCAL_LOCATION: - break; case MSG_GPS_RAW: break; case MSG_SERVO_OUT: diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 9923ab8f98..32ee10ec50 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -149,20 +149,6 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) crosstrack_error); } -static void NOINLINE send_local_location(mavlink_channel_t chan) -{ - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_local_position_send( - chan, - micros(), - ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, - ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), - (current_loc.alt - home.alt) / 1.0e2, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); -} - static void NOINLINE send_gps_raw(mavlink_channel_t chan) { mavlink_msg_gps_raw_send( @@ -348,11 +334,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } break; - case MSG_LOCAL_LOCATION: - CHECK_PAYLOAD_SIZE(LOCAL_POSITION); - send_local_location(chan); - break; - case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW); send_gps_raw(chan); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 1f638da505..7d0aac6c2b 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -163,7 +163,6 @@ #define MSG_ATTITUDE_CORRECT 0xb1 #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 -#define MSG_LOCAL_LOCATION 0xb4 #define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1 From f74c1f9c7eff9e06f90cd57521c91483050aa2f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 11:35:55 +1000 Subject: [PATCH 100/100] MAVLink: merged in the stack saving changes from ArduPlane --- ArduCopter/HIL_Xplane.pde | 2 - ArduCopter/Mavlink_Common.h | 593 +++++++++++++++++++----------------- ArduCopter/defines.h | 1 - 3 files changed, 314 insertions(+), 282 deletions(-) diff --git a/ArduCopter/HIL_Xplane.pde b/ArduCopter/HIL_Xplane.pde index 311b9da649..9ecf47ab84 100644 --- a/ArduCopter/HIL_Xplane.pde +++ b/ArduCopter/HIL_Xplane.pde @@ -93,8 +93,6 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param) break; case MSG_LOCATION: break; - case MSG_LOCAL_LOCATION: - break; case MSG_GPS_RAW: break; case MSG_SERVO_OUT: diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index 6ecb79761f..927289dad6 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -25,14 +25,257 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +#define NOINLINE __attribute__((noinline)) + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + mavlink_msg_attitude_send( + chan, + micros(), + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); +} + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_HOLD; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; + default: + mode = control_mode + 100; + } + + uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + 0, + battery_voltage * 1000, + battery_remaining, + packet_drops); +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + target_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + 0, + 0); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + mavlink_msg_rc_channels_scaled_send( + chan, + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), + 0, + 0, + 0, + 0, + rssi); +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.rc_1.radio_in, + g.rc_2.radio_in, + g.rc_3.radio_in, + g.rc_4.radio_in, + g.rc_5.radio_in, + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + motor_out[0], + motor_out[1], + motor_out[2], + motor_out[3], + motor_out[4], + motor_out[5], + motor_out[6], + motor_out[7]); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + g.rc_3.servo_out/10, + current_loc.alt / 100.0, + climb_rate); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + 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()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); +} + // 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 @@ -40,298 +283,90 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } switch(id) { + case MSG_HEARTBEAT: + CHECK_PAYLOAD_SIZE(HEARTBEAT); + send_heartbeat(chan); + break; - case MSG_HEARTBEAT: - { - CHECK_PAYLOAD_SIZE(HEARTBEAT); - mavlink_msg_heartbeat_send( - chan, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; - } + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; - case MSG_EXTENDED_STATUS1: - { - CHECK_PAYLOAD_SIZE(SYS_STATUS); + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; - switch(control_mode) { - case LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_HOLD; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case GUIDED: - mode = MAV_MODE_GUIDED; - break; - default: - mode = control_mode + 100; + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; - } + case MSG_NAV_CONTROLLER_OUTPUT: + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); + break; - uint8_t status = MAV_STATE_ACTIVE; - uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + case MSG_GPS_RAW: + CHECK_PAYLOAD_SIZE(GPS_RAW); + send_gps_raw(chan); + break; - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - 0, - battery_voltage * 1000, - battery_remaining, - packet_drops); - break; - } + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; - case MSG_EXTENDED_STATUS2: - { - CHECK_PAYLOAD_SIZE(MEMINFO); - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); - break; - } + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; - case MSG_ATTITUDE: - { - //Vector3f omega = dcm.get_gyro(); - CHECK_PAYLOAD_SIZE(ATTITUDE); - mavlink_msg_attitude_send( - chan, - timeStamp, - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); - break; - } + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; - 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, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, // reverted to relative altitude - /*g_gps->altitude,*/ - g_gps->ground_speed * rot.a.x, - g_gps->ground_speed * rot.b.x, - g_gps->ground_speed * rot.c.x); - break; - } + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; - 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, - nav_pitch / 1.0e2, - target_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - 0, - 0); - //} - break; - } +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; - 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, - timeStamp, - ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, - ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), - (current_loc.alt - home.alt) / 1.0e2, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); - break; - } + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; - case MSG_GPS_RAW: - { - CHECK_PAYLOAD_SIZE(GPS_RAW); - mavlink_msg_gps_raw_send( - chan, - timeStamp, - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); - break; - } + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE - 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 - mavlink_msg_rc_channels_scaled_send( - chan, - 10000 * g.rc_1.norm_output(), - 10000 * g.rc_2.norm_output(), - 10000 * g.rc_3.norm_output(), - 10000 * g.rc_4.norm_output(), - 0, - 0, - 0, - 0, - rssi); - break; - } + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; - case MSG_RADIO_IN: - { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - uint8_t rssi = 1; - mavlink_msg_rc_channels_raw_send( - chan, - g.rc_1.radio_in, - g.rc_2.radio_in, - g.rc_3.radio_in, - g.rc_4.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); - break; - } + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; - case MSG_RADIO_OUT: - { - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - mavlink_msg_servo_output_raw_send( - chan, - motor_out[0], - motor_out[1], - motor_out[2], - motor_out[3], - motor_out[4], - motor_out[5], - motor_out[6], - motor_out[7]); - break; - } - - case MSG_VFR_HUD: - { - CHECK_PAYLOAD_SIZE(VFR_HUD); - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, - g.rc_3.servo_out/10, - current_loc.alt / 100.0, - /*g_gps->altitude/100.0,*/ // reverted to relative altitude - climb_rate); - break; - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - 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( - chan, - timeStamp, - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); - break; - } - - case MSG_RAW_IMU2: - { - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - mavlink_msg_scaled_pressure_send( - chan, - timeStamp, - (float)barometer.Press/100.0, - (float)(barometer.Press-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, - NULL, - NULL, - NULL, - NULL, - NULL); - break; - } - - case MSG_CURRENT_WAYPOINT: - { - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); - mavlink_msg_waypoint_current_send( - chan, - g.waypoint_index); - break; - } - - default: - break; + default: + break; } return true; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7f1c1dcf62..eca5b9ef6e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -243,7 +243,6 @@ #define MSG_ATTITUDE_CORRECT 0xb1 #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 -#define MSG_LOCAL_LOCATION 0xb4 #define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1