From 609ae8359de6d76003baca82393dc7954e9ca1b0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:45:13 +1000 Subject: [PATCH 01/31] import APM_Camera branch from SVN --- ArduPlane/APM_Config.h | 38 +- ArduPlane/APM_Config.h.reference | 36 +- ArduPlane/ArduPilotMega.pde | 361 +++++++------ ArduPlane/Attitude.pde | 139 ++--- ArduPlane/Cam_move.pde | 210 ++++++++ ArduPlane/Cam_trigger.pde | 41 ++ ArduPlane/GCS.h | 8 +- ArduPlane/GCS_Mavlink.pde | 484 ++++++----------- ArduPlane/HIL.h | 8 +- ArduPlane/HIL_Xplane.pde | 2 +- ArduPlane/Log.pde | 107 ++-- ArduPlane/Mavlink_Common.h | 158 +----- ArduPlane/Parameters.h | 844 ++++++++++++++---------------- ArduPlane/WP_activity.pde | 49 ++ ArduPlane/climb_rate.pde | 44 +- ArduPlane/command description.txt | 2 +- ArduPlane/commands.pde | 103 ++-- ArduPlane/commands_logic.pde | 108 ++-- ArduPlane/commands_process.pde | 20 +- ArduPlane/config.h | 117 +---- ArduPlane/control_modes.pde | 73 ++- ArduPlane/defines.h | 61 +-- ArduPlane/events.pde | 26 +- ArduPlane/navigation.pde | 57 +- ArduPlane/planner.pde | 9 +- ArduPlane/radio.pde | 66 +-- ArduPlane/sensors.pde | 40 +- ArduPlane/setup.pde | 145 ++--- ArduPlane/system.pde | 170 +++--- ArduPlane/test.pde | 135 +++-- 30 files changed, 1707 insertions(+), 1954 deletions(-) create mode 100644 ArduPlane/Cam_move.pde create mode 100644 ArduPlane/Cam_trigger.pde create mode 100644 ArduPlane/WP_activity.pde diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 3a8bbb504f..4006d33839 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,5 +1,4 @@ - - // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // their default values, place the appropriate #define statements here. @@ -22,4 +21,37 @@ #define HIL_PORT 0 #define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #define GCS_PORT 3 -*/ +*/ + +#define MAGNETOMETER ENABLED + +// ----- Camera definitions ------ +// ------------------------------ +#define CAMERA ENABLED +#define CAM_DEBUG DISABLED +// Comment out servos that you do not have +//#define CAM_SERVO 8 // Camera servo channel +#define CAM_ANGLE 30 // Set angle in degrees +//#define CAM_CLICK 45 // This is the position of the servo arm when it actually clicks +//#define OPEN_SERVO 5 // Retraction servo channel - my camera retracts yours might not. + +// Camera yaw (left-right) +#define YAW_SERVO 7 // Pan servo channel (can be pitch in stabilization) +#define YAW_REV 1 // output is normal = 1 output is reversed = -1 +#define YAW_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg +#define YAW_RANGE 90 // Pan Servo sweep in degrees +#define YAW_RATIO 10.31 // match this to the swing of your pan servo + +// Camera pitch (up-down) +#define PITCH_SERVO 6 // Tilt servo channel (can be roll in stabilization) +#define PITCH_REV 1 // output is normal = 1 output is reversed = -1 +#define PITCH_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg +#define PITCH_RANGE 90 // Tilt Servo sweep in degrees +#define PITCH_RATIO 10.31 // match this to the swing of your tilt servo + +// Camera roll (up-down) +#define ROLL_SERVO 6 // Tilt servo channel (can be roll in stabilization) +#define ROLL_REV 1 // output is normal = 1 output is reversed = -1 +#define ROLL_CENTER 0 // Camera center bearing with relation to airframe forward motion - deg +#define ROLL_RANGE 90 // Tilt Servo sweep in degrees +#define ROLL_RATIO 10.31 // match this to the swing of your tilt servo diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 6d8685b3e3..18fa27d019 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -210,7 +210,7 @@ // INPUT_VOLTAGE OPTIONAL // // In order to have accurate pressure and battery voltage readings, this -// value should be set to the voltage measured at the processor. +// value should be set to the voltage measured on the 5V rail on the oilpan. // // See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail. // @@ -304,32 +304,6 @@ //#define FLIGHT_MODE_6 MANUAL // -////////////////////////////////////////////////////////////////////////////// -// RC_5_FUNCT OPTIONAL -// RC_6_FUNCT OPTIONAL -// RC_7_FUNCT OPTIONAL -// RC_8_FUNCT OPTIONAL -// -// The channel 5 through 8 function assignments allow configuration of those -// channels for use with differential ailerons, flaps, flaperons, or camera -// or intrument mounts -// -//#define RC_5_FUNCT RC_5_FUNCT_NONE -//etc. - -////////////////////////////////////////////////////////////////////////////// -// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed -// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed -// then the flap position shall be flap_1_percent. If the speed setpoint is below flap_2_speed -// then the flap position shall be flap_2_percent. Speed setpoint is the current value of -// airspeed_cruise (if airspeed sensor enabled) or throttle_cruise (if no airspeed sensor) - -// FLAP_1_PERCENT OPTIONAL -// FLAP_1_SPEED OPTIONAL -// FLAP_2_PERCENT OPTIONAL -// FLAP_2_SPEED OPTIONAL - - ////////////////////////////////////////////////////////////////////////////// // THROTTLE_FAILSAFE OPTIONAL // THROTTLE_FS_VALUE OPTIONAL @@ -387,7 +361,7 @@ // the aircraft will head for home in RTL mode. If the failsafe condition is // resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home -// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER +// If XX_FAILSAFE_ACTION is 2 and the applicable failsafe occurs while in AUTO or LOITER // mode the aircraft will continue in that mode ignoring the failsafe condition. // Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always @@ -398,8 +372,8 @@ // The default behaviour is to ignore failsafes in AUTO and LOITER modes. // //#define GCS_HEARTBEAT_FAILSAFE DISABLED -//#define SHORT_FAILSAFE_ACTION 0 -//#define LONG_FAILSAFE_ACTION 0 +//#define SHORT_FAILSAFE_ACTION 2 +//#define LONG_FAILSAFE_ACTION 2 ////////////////////////////////////////////////////////////////////////////// @@ -800,7 +774,7 @@ // // Limits the slew rate of the throttle, in percent per second. Helps // avoid sudden throttle changes, which can destabilise the aircraft. -// A setting of zero disables the feature. Range 1 to 100. +// A setting of zero disables the feature. // Default is zero (disabled). // // P_TO_T OPTIONAL diff --git a/ArduPlane/ArduPilotMega.pde b/ArduPlane/ArduPilotMega.pde index 6f2d3dd601..8a43a61634 100644 --- a/ArduPlane/ArduPilotMega.pde +++ b/ArduPlane/ArduPilotMega.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.23" +#define THISFIRMWARE "ArduPilotMega 2.2.0" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -29,7 +29,6 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega RC Library #include // ArduPilot GPS library #include // Arduino I2C lib -#include // Arduino SPI lib #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library #include // ArduPilot Mega BMP085 Library @@ -39,10 +38,10 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library -#include // Range finder library -#include +//#include // Range finder library + +#define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions -#include // Configuration #include "config.h" @@ -71,12 +70,12 @@ FastSerialPort3(Serial3); // Telemetry port // // Global parameters are all contained within the 'g' class. // -static Parameters g; +Parameters g; //////////////////////////////////////////////////////////////////////////////// // prototypes -static void update_events(void); +void update_events(void); //////////////////////////////////////////////////////////////////////////////// @@ -93,17 +92,23 @@ static void update_events(void); // // All GPS access should be through this pointer. -static GPS *g_gps; - -// flight modes convenience array -static AP_Int8 *flight_modes = &g.flight_mode1; +GPS *g_gps; #if HIL_MODE == HIL_MODE_DISABLED // real sensors -static AP_ADC_ADS7844 adc; -static APM_BMP085_Class barometer; -static AP_Compass_HMC5843 compass(Parameters::k_param_compass); +AP_ADC_ADS7844 adc; +APM_BMP085_Class barometer; +// MAG PROTOCOL +#if MAG_PROTOCOL == MAG_PROTOCOL_5843 +AP_Compass_HMC5843 compass(Parameters::k_param_compass); + +#elif MAG_PROTOCOL == MAG_PROTOCOL_5883L +AP_Compass_HMC5883L compass(Parameters::k_param_compass); + +#else + #error Unrecognised MAG_PROTOCOL setting. +#endif // MAG PROTOCOL // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -139,7 +144,6 @@ AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); #elif HIL_MODE == HIL_MODE_ATTITUDE -AP_ADC_HIL adc; AP_DCM_HIL dcm; AP_GPS_HIL g_gps_driver(NULL); AP_Compass_HIL compass; // never used @@ -185,31 +189,18 @@ AP_IMU_Shim imu; // never used GCS_Class gcs; #endif -//////////////////////////////////////////////////////////////////////////////// -// SONAR selection -//////////////////////////////////////////////////////////////////////////////// -// -ModeFilter sonar_mode_filter; - -#if SONAR_TYPE == MAX_SONAR_XL - AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); -#elif SONAR_TYPE == MAX_SONAR_LV - // XXX honestly I think these output the same values - // If someone knows, can they confirm it? - AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc); -#endif +//AP_RangeFinder_MaxsonarXL sonar; //////////////////////////////////////////////////////////////////////////////// // Global variables //////////////////////////////////////////////////////////////////////////////// -byte control_mode = INITIALISING; +byte control_mode = MANUAL; byte oldSwitchPosition; // for remembering the control mode switch -bool inverted_flight = false; -static const char *comma = ","; +const char *comma = ","; -static const char* flight_mode_strings[] = { +const char* flight_mode_strings[] = { "Manual", "Circle", "Stabilize", @@ -241,179 +232,214 @@ static const char* flight_mode_strings[] = { // Failsafe // -------- -static int failsafe; // track which type of failsafe is being processed -static bool ch3_failsafe; -static byte crash_timer; +int failsafe; // track which type of failsafe is being processed +bool ch3_failsafe; +byte crash_timer; // Radio // ----- -static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm -static uint16_t elevon2_trim = 1500; -static uint16_t ch1_temp = 1500; // Used for elevon mixing -static uint16_t ch2_temp = 1500; -static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; -static bool rc_override_active = false; -static uint32_t rc_override_fs_timer = 0; -static uint32_t ch3_failsafe_timer = 0; +uint16_t elevon1_trim = 1500; // TODO: handle in EEProm +uint16_t elevon2_trim = 1500; +uint16_t ch1_temp = 1500; // Used for elevon mixing +uint16_t ch2_temp = 1500; +int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; +bool rc_override_active = false; +uint32_t rc_override_fs_timer = 0; +uint32_t ch3_failsafe_timer = 0; +bool reverse_roll; +bool reverse_pitch; +bool reverse_rudder; +byte mix_mode; // 0 = normal , 1 = elevons + +// TODO: switch these reverses to true/false, after they are handled by RC_Channel +int reverse_elevons = 1; +int reverse_ch1_elevon = 1; +int reverse_ch2_elevon = 1; // for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon // LED output // ---------- -static bool GPS_light; // status of the GPS light +bool GPS_light; // status of the GPS light // GPS variables // ------------- -static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage -static float scaleLongUp = 1; // used to reverse longitude scaling -static float scaleLongDown = 1; // used to reverse longitude scaling -static byte ground_start_count = 5; // have we achieved first lock and set Home? -static int ground_start_avg; // 5 samples to avg speed for ground start -static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present +const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage +float scaleLongUp = 1; // used to reverse longtitude scaling +float scaleLongDown = 1; // used to reverse longtitude scaling +byte ground_start_count = 5; // have we achieved first lock and set Home? +int ground_start_avg; // 5 samples to avg speed for ground start +bool ground_start; // have we started on the ground? +bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present // Location & Navigation // --------------------- const float radius_of_earth = 6378100; // meters const float gravity = 9.81; // meters/ sec^2 -static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate -static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target -static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate -static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? -static long hold_course = -1; // deg * 100 dir of plane +long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target +long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate +float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? +long hold_course = -1; // deg * 100 dir of plane -static byte command_must_index; // current command memory location -static byte command_may_index; // current command memory location -static byte command_must_ID; // current command ID -static byte command_may_ID; // current command ID +byte command_must_index; // current command memory location +byte command_may_index; // current command memory location +byte command_must_ID; // current command ID +byte command_may_ID; // current command ID // Airspeed // -------- -static int airspeed; // m/s * 100 -static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -static float airspeed_error; // m/s * 100 -static long energy_error; // energy state error (kinetic + potential) for altitude hold -static long airspeed_energy_error; // kinetic portion of energy error +int airspeed; // m/s * 100 +int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range +float airspeed_error; // m/s * 100 +long energy_error; // energy state error (kinetic + potential) for altitude hold +long airspeed_energy_error; // kinetic portion of energy error +bool airspeed_enabled = false; // Location Errors // --------------- -static long bearing_error; // deg * 100 : 0 to 36000 -static long altitude_error; // meters * 100 we are off in altitude -static float crosstrack_error; // meters we are off trackline +long bearing_error; // deg * 100 : 0 to 36000 +long altitude_error; // meters * 100 we are off in altitude +float crosstrack_error; // meters we are off trackline // Battery Sensors // --------------- -static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter -static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter -static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter +float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter +float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter +float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter +float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter +float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter -static float current_amps; -static float current_total; +float current_amps; +float current_total; // Airspeed Sensors // ---------------- -static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering -static int airspeed_pressure; // airspeed as a pressure value +float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering +int airspeed_offset; // analog air pressure sensor while still +int airspeed_pressure; // airspeed as a pressure value // Barometer Sensor variables // -------------------------- -static unsigned long abs_pressure; +unsigned long abs_pressure; // Altitude Sensor variables // ---------------------- -static int sonar_alt; +//byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR // flight mode specific // -------------------- -static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. -static bool land_complete; -static long takeoff_altitude; -// static int landing_distance; // meters; -static int landing_pitch; // pitch for landing set by commands -static int takeoff_pitch; +bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes. +bool land_complete; +long takeoff_altitude; +int landing_distance; // meters; +int landing_pitch; // pitch for landing set by commands +int takeoff_pitch; // Loiter management // ----------------- -static long old_target_bearing; // deg * 100 -static int loiter_total; // deg : how many times to loiter * 360 -static int loiter_delta; // deg : how far we just turned -static int loiter_sum; // deg : how far we have turned around a waypoint -static long loiter_time; // millis : when we started LOITER mode -static int loiter_time_max; // millis : how long to stay in LOITER mode +long old_target_bearing; // deg * 100 +int loiter_total; // deg : how many times to loiter * 360 +int loiter_delta; // deg : how far we just turned +int loiter_sum; // deg : how far we have turned around a waypoint +long loiter_time; // millis : when we started LOITER mode +int loiter_time_max; // millis : how long to stay in LOITER mode // these are the values for navigation control functions // ---------------------------------------------------- -static long nav_roll; // deg * 100 : target roll angle -static long nav_pitch; // deg * 100 : target pitch angle -static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel +long nav_roll; // deg * 100 : target roll angle +long nav_pitch; // deg * 100 : target pitch angle +int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel // Waypoints // --------- -static long wp_distance; // meters - distance between plane and next waypoint -static long wp_totalDistance; // meters - distance between old and next waypoint +long wp_distance; // meters - distance between plane and next waypoint +long wp_totalDistance; // meters - distance between old and next waypoint +byte next_wp_index; // Current active command index // repeating event control // ----------------------- -static byte event_id; // what to do - see defines -static long event_timer; // when the event was asked for in ms -static uint16_t event_delay; // how long to delay the next firing of event in millis -static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles -static int event_value; // per command value, such as PWM for servos -static int event_undo_value; // the value used to cycle events (alternate value to event_value) +byte event_id; // what to do - see defines +long event_timer; // when the event was asked for in ms +uint16_t event_delay; // how long to delay the next firing of event in millis +int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles +int event_value; // per command value, such as PWM for servos +int event_undo_value; // the value used to cycle events (alternate value to event_value) // delay command // -------------- -static long condition_value; // used in condition commands (eg delay, change alt, etc.) -static long condition_start; -static int condition_rate; +long condition_value; // used in condition commands (eg delay, change alt, etc.) +long condition_start; +int condition_rate; // 3D Location vectors // ------------------- -static struct Location home; // home location -static struct Location prev_WP; // last waypoint -static struct Location current_loc; // current location -static struct Location next_WP; // next waypoint -static struct Location next_command; // command preloaded -static struct Location guided_WP; // guided mode waypoint -static long target_altitude; // used for altitude management between waypoints -static long offset_altitude; // used for altitude management between waypoints -static bool home_is_set; // Flag for if we have g_gps lock and have set the home location +struct Location home; // home location +struct Location prev_WP; // last waypoint +struct Location current_loc; // current location +struct Location next_WP; // next waypoint +struct Location next_command; // command preloaded +long target_altitude; // used for altitude management between waypoints +long offset_altitude; // used for altitude management between waypoints +bool home_is_set; // Flag for if we have g_gps lock and have set the home location // IMU variables // ------------- -static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) +float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) // Performance monitoring // ---------------------- -static long perf_mon_timer; // Metric based on accel gain deweighting -static int G_Dt_max = 0; // Max main loop cycle time in milliseconds -static int gps_fix_count = 0; -static int pmTest1 = 0; +long perf_mon_timer; // Metric based on accel gain deweighting +int G_Dt_max; // Max main loop cycle time in milliseconds +int gps_fix_count; +byte gcs_messages_sent; +// GCS +// --- +char GCS_buffer[53]; +char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed + // System Timers // -------------- -static unsigned long fast_loopTimer; // Time in miliseconds of main control loop -static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete -static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -static int mainLoop_count; +unsigned long fast_loopTimer; // Time in miliseconds of main control loop +unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete +uint8_t delta_ms_fast_loop; // Delta Time in miliseconds +int mainLoop_count; -static unsigned long medium_loopTimer; // Time in miliseconds of medium loop -static byte medium_loopCounter; // Counters for branching from main control loop to slower loops -static uint8_t delta_ms_medium_loop; +unsigned long medium_loopTimer; // Time in miliseconds of medium loop +byte medium_loopCounter; // Counters for branching from main control loop to slower loops +uint8_t delta_ms_medium_loop; -static byte slow_loopCounter; -static byte superslow_loopCounter; -static byte counter_one_herz; +byte slow_loopCounter; +byte superslow_loopCounter; +byte counter_one_herz; -static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav +unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav -static unsigned long dTnav; // Delta Time in milliseconds for navigation computations -static float load; // % MCU cycles used +unsigned long dTnav; // Delta Time in milliseconds for navigation computations +unsigned long elapsedTime; // for doing custom events +float load; // % MCU cycles used + +//Camera tracking and stabilisation stuff +// -------------------------------------- +byte camera_mode = 1; //0 is do nothing, 1 is stabilize, 2 is track target +byte gimbal_mode = 0; // 0 - pitch & roll, 1 - pitch and yaw (pan & tilt), 2 - pitch, roll and yaw (to be added) +struct Location camera_target; //point of iterest for the camera to track +Vector3 target_vector(0,0,1); //x, y, z to target before rotating to planes axis, values are in meters +float cam_pitch; +float cam_roll; +float cam_tilt; +float cam_pan; + +struct Location GPS_mark; // GPS POI for position based triggering +int picture_time = 0; // waypoint trigger variable +int thr_pic = 0; // timer variable for throttle_pic +int camtrig = 83; // PK6 chosen as it not near anything so safer for soldering +pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) //////////////////////////////////////////////////////////////////////////////// @@ -421,7 +447,6 @@ static float load; // % MCU cycles used //////////////////////////////////////////////////////////////////////////////// void setup() { - memcheck_init(); init_ardupilot(); } @@ -466,7 +491,7 @@ void loop() } // Main loop 50Hz -static void fast_loop() +void fast_loop() { // This is the fast loop - we want it to execute at 50Hz if possible // ----------------------------------------------------------------- @@ -477,22 +502,15 @@ static void fast_loop() // ---------- read_radio(); - // try to send any deferred messages if the serial port now has - // some space available - gcs.send_message(MSG_RETRY_DEFERRED); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.send_message(MSG_RETRY_DEFERRED); -#endif - // check for loss of control signal failsafe condition // ------------------------------------ check_short_failsafe(); // Read Airspeed // ------------- - if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { + if (airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) { read_airspeed(); - } else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { + } else if (airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) { calc_airspeed_errors(); } @@ -532,7 +550,7 @@ static void fast_loop() // write out the servo PWM values // ------------------------------ - set_servos(); + set_servos_4(); // XXX is it appropriate to be doing the comms below on the fast loop? @@ -561,8 +579,9 @@ static void fast_loop() // or be a "GCS fast loop" interface } -static void medium_loop() +void medium_loop() { + camera(); // This is the start of the medium (10 Hz) loop pieces // ----------------------------------------- switch(medium_loopCounter) { @@ -595,6 +614,7 @@ Serial.println(tempaccel.z, DEC); // This case performs some navigation computations //------------------------------------------------ case 1: + medium_loopCounter++; @@ -618,7 +638,6 @@ Serial.println(tempaccel.z, DEC); // Read altitude from sensors // ------------------ update_alt(); - if(g.sonar_enabled) sonar_alt = sonar.read(); // altitude smoothing // ------------------ @@ -679,7 +698,7 @@ Serial.println(tempaccel.z, DEC); } } -static void slow_loop() +void slow_loop() { // This is the slow (3 1/3 Hz) loop pieces //---------------------------------------- @@ -708,7 +727,9 @@ static void slow_loop() // Read Control Surfaces/Mix switches // ---------------------------------- - update_servo_switches(); + if (g.switch_enable) { + update_servo_switches(); + } break; @@ -716,18 +737,19 @@ static void slow_loop() slow_loopCounter = 0; update_events(); + // XXX this should be a "GCS slow loop" interface #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter - gcs.data_stream_send(3,5); + gcs.data_stream_send(1,5); // send all requested output streams with rates requested - // between 3 and 5 Hz + // between 1 and 5 Hz #else gcs.send_message(MSG_LOCATION); gcs.send_message(MSG_CPU_LOAD, load*100); #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(3,5); + hil.data_stream_send(1,5); #endif @@ -735,26 +757,20 @@ static void slow_loop() } } -static void one_second_loop() +void one_second_loop() { if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); // send a heartbeat gcs.send_message(MSG_HEARTBEAT); - #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(1,3); - // send all requested output streams with rates requested - // between 1 and 3 Hz - #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.send_message(MSG_HEARTBEAT); - hil.data_stream_send(1,3); #endif } -static void update_GPS(void) +void update_GPS(void) { g_gps->update(); update_GPS_light(); @@ -806,7 +822,7 @@ static void update_GPS(void) } } -static void update_current_flight_mode(void) +void update_current_flight_mode(void) { if(control_mode == AUTO){ crash_checker(); @@ -819,7 +835,7 @@ static void update_current_flight_mode(void) nav_roll = 0; } - if (g.airspeed_enabled == true) + if (airspeed_enabled == true) { calc_nav_pitch(); if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; @@ -837,7 +853,7 @@ static void update_current_flight_mode(void) case MAV_CMD_NAV_LAND: calc_nav_roll(); - if (g.airspeed_enabled == true){ + if (airspeed_enabled == true){ calc_nav_pitch(); calc_throttle(); }else{ @@ -862,7 +878,6 @@ static void update_current_flight_mode(void) switch(control_mode){ case RTL: case LOITER: - case GUIDED: hold_course = -1; crash_checker(); calc_nav_roll(); @@ -876,7 +891,6 @@ static void update_current_flight_mode(void) nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min; // We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign. nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority - if (inverted_flight) nav_pitch = -nav_pitch; break; case FLY_BY_WIRE_B: @@ -886,7 +900,7 @@ static void update_current_flight_mode(void) nav_roll = g.channel_roll.norm_input() * g.roll_limit; altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; - if (g.airspeed_enabled == true) + if (airspeed_enabled == true) { airspeed_error = ((int)(g.flybywire_airspeed_max - g.flybywire_airspeed_min) * @@ -935,7 +949,7 @@ static void update_current_flight_mode(void) } } -static void update_navigation() +void update_navigation() { // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS // ------------------------------------------------------------------------ @@ -947,18 +961,23 @@ static void update_navigation() switch(control_mode){ case LOITER: - case RTL: - case GUIDED: update_loiter(); calc_bearing_error(); break; + case RTL: + if(wp_distance <= ( g.loiter_radius + LOITER_RANGE) ) { + do_RTL(); + }else{ + update_crosstrack(); + } + break; } } } -static void update_alt() +void update_alt() { #if HIL_MODE == HIL_MODE_ATTITUDE current_loc.alt = g_gps->altitude; @@ -971,6 +990,6 @@ static void update_alt() #endif // Calculate new climb rate - //if(medium_loopCounter == 0 && slow_loopCounter == 0) - // add_altitude_data(millis() / 100, g_gps->altitude / 10); + if(medium_loopCounter == 0 && slow_loopCounter == 0) + add_altitude_data(millis() / 100, g_gps->altitude / 10); } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 3305927fa4..b942ec759d 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -4,14 +4,15 @@ // Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed. //**************************************************************** -static void stabilize() +void stabilize() { + static byte temp = 0; float ch1_inf = 1.0; float ch2_inf = 1.0; float ch4_inf = 1.0; float speed_scaler; - if (g.airspeed_enabled == true){ + if (airspeed_enabled == true){ if(airspeed > 0) speed_scaler = (STANDARD_SPEED * 100) / airspeed; else @@ -19,7 +20,7 @@ static void stabilize() speed_scaler = constrain(speed_scaler, 0.5, 2.0); } else { if (g.channel_throttle.servo_out > 0){ - speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root + speed_scaler = 0.5 + (THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root // Should maybe be to the 2/7 power, but we aren't goint to implement that... }else{ speed_scaler = 1.67; @@ -31,16 +32,6 @@ static void stabilize() nav_roll = 0; } - if (inverted_flight) { - // we want to fly upside down. We need to cope with wrap of - // the roll_sensor interfering with wrap of nav_roll, which - // would really confuse the PID code. The easiest way to - // handle this is to ensure both go in the same direction from - // zero - nav_roll += 18000; - if (dcm.roll_sensor < 0) nav_roll -= 36000; - } - // For Testing Only // roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10; // Serial.printf_P(PSTR(" roll_sensor ")); @@ -53,10 +44,6 @@ static void stabilize() fabs(dcm.roll_sensor * g.kff_pitch_compensation) + (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - (dcm.pitch_sensor - g.pitch_trim); - if (inverted_flight) { - // when flying upside down the elevator control is inverted - tempcalc = -tempcalc; - } g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); // Mix Stick input to allow users to override control surfaces @@ -112,7 +99,7 @@ static void stabilize() //#endif } -static void crash_checker() +void crash_checker() { if(dcm.pitch_sensor < -4500){ crash_timer = 255; @@ -122,9 +109,9 @@ static void crash_checker() } -static void calc_throttle() +void calc_throttle() { - if (g.airspeed_enabled == false) { + if (airspeed_enabled == false) { int throttle_target = g.throttle_cruise + throttle_nudge; // no airspeed sensor, we use nav pitch to determine the proper throttle output @@ -158,7 +145,7 @@ static void calc_throttle() // Yaw is separated into a function for future implementation of heading hold on rolling take-off // ---------------------------------------------------------------------------------------- -static void calc_nav_yaw(float speed_scaler) +void calc_nav_yaw(float speed_scaler) { #if HIL_MODE != HIL_MODE_ATTITUDE Vector3f temp = imu.get_accel(); @@ -173,11 +160,11 @@ static void calc_nav_yaw(float speed_scaler) } -static void calc_nav_pitch() +void calc_nav_pitch() { // Calculate the Pitch of the plane // -------------------------------- - if (g.airspeed_enabled == true) { + if (airspeed_enabled == true) { nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); } else { nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); @@ -188,7 +175,7 @@ static void calc_nav_pitch() #define YAW_DAMPENER 0 -static void calc_nav_roll() +void calc_nav_roll() { // Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. @@ -232,22 +219,18 @@ float roll_slew_limit(float servo) /***************************************** * Throttle slew limit *****************************************/ -static void throttle_slew_limit() +/*float throttle_slew_limit(float throttle) { - static int last = 1000; - if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit - - float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000 -Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last); - g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp); - last = g.channel_throttle.radio_out; - } + static float last; + float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + THROTTLE_SLEW_LIMIT * delta_ms_fast_loop/1000.f); + last = throttle; + return temp; } - +*/ // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -static void reset_I(void) +void reset_I(void) { g.pidNavRoll.reset_I(); g.pidNavPitchAirspeed.reset_I(); @@ -259,13 +242,11 @@ static void reset_I(void) /***************************************** * Set the flight control servos based on the current calculated values *****************************************/ -static void set_servos(void) +void set_servos_4(void) { - int flapSpeedSource = 0; - if(control_mode == MANUAL){ // do a direct pass through of radio values - if (g.mix_mode == 0){ + if (mix_mode == 0){ g.channel_roll.radio_out = g.channel_roll.radio_in; g.channel_pitch.radio_out = g.channel_pitch.radio_in; } else { @@ -274,31 +255,21 @@ static void set_servos(void) } g.channel_throttle.radio_out = g.channel_throttle.radio_in; g.channel_rudder.radio_out = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in; - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in; } else { - if (g.mix_mode == 0) { + if (mix_mode == 0){ g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); g.channel_rudder.calc_pwm(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.servo_out = g.channel_roll.servo_out; - g.rc_5.calc_pwm(); - } - if (g.rc_6_funct == RC_6_FUNCT_AILERON) { - g.rc_6.servo_out = g.channel_roll.servo_out; - g.rc_6.calc_pwm(); - } }else{ /*Elevon mode*/ float ch1; float ch2; - ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out); + ch1 = reverse_elevons * (g.channel_pitch.servo_out - g.channel_roll.servo_out); ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out; - g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); - g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); + g.channel_roll.radio_out = elevon1_trim + (reverse_ch1_elevon * (ch1 * 500.0/ ROLL_SERVO_MAX)); + g.channel_pitch.radio_out = elevon2_trim + (reverse_ch2_elevon * (ch2 * 500.0/ PITCH_SERVO_MAX)); } #if THROTTLE_OUT == 0 @@ -310,61 +281,55 @@ static void set_servos(void) g.channel_throttle.calc_pwm(); + //Radio_in: 1763 PWM output: 2000 Throttle: 78.7695999145 PWM Min: 1110 PWM Max: 1938 + /* TO DO - fix this for RC_Channel library #if THROTTLE_REVERSE == 1 radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE]; #endif */ - - throttle_slew_limit(); } - - if(control_mode <= FLY_BY_WIRE_B) { - if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in; - if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in; - } else if (control_mode >= FLY_BY_WIRE_C) { - if (g.airspeed_enabled == true) { - flapSpeedSource = g.airspeed_cruise; - } else { - flapSpeedSource = g.throttle_cruise; - } - if ( flapSpeedSource > g.flap_1_speed) { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0; - } else if (flapSpeedSource > g.flap_2_speed) { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent; - } else { - if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent; - if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent; - } - } - -#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS + + // send values to the PWM timers for output // ---------------------------------------- APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos - APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos - APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos -#endif } -static void demo_servos(byte i) { +void demo_servos(byte i) { while(i > 0){ gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); -#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS APM_RC.OutputCh(1, 1400); - mavlink_delay(400); + delay(400); APM_RC.OutputCh(1, 1600); - mavlink_delay(200); + delay(200); APM_RC.OutputCh(1, 1500); -#endif - mavlink_delay(400); + delay(400); i--; } } +int readOutputCh(unsigned char ch) +{ + int pwm; + switch(ch) + { + case 0: pwm = OCR5B; break; // ch0 + case 1: pwm = OCR5C; break; // ch1 + case 2: pwm = OCR1B; break; // ch2 + case 3: pwm = OCR1C; break; // ch3 + case 4: pwm = OCR4C; break; // ch4 + case 5: pwm = OCR4B; break; // ch5 + case 6: pwm = OCR3C; break; // ch6 + case 7: pwm = OCR3B; break; // ch7 + case 8: pwm = OCR5A; break; // ch8, PL3 + case 9: pwm = OCR1A; break; // ch9, PB5 + case 10: pwm = OCR3A; break; // ch10, PE3 + } + pwm >>= 1; // pwm / 2; + return pwm; +} diff --git a/ArduPlane/Cam_move.pde b/ArduPlane/Cam_move.pde new file mode 100644 index 0000000000..bc8ad0eac7 --- /dev/null +++ b/ArduPlane/Cam_move.pde @@ -0,0 +1,210 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if CAMERA == ENABLED + +void init_camera() +{ + g.rc_camera_pitch.set_angle(4500); // throw of servo? + g.rc_camera_pitch.radio_min = 1000; // lowest radio input + g.rc_camera_pitch.radio_trim = 1500; // middle radio input + g.rc_camera_pitch.radio_max = 2000; // highest radio input + + g.rc_camera_roll.set_angle(4500); + g.rc_camera_roll.radio_min = 1000; + g.rc_camera_roll.radio_trim = 1500; + g.rc_camera_roll.radio_max = 2000; + + + //use test target for now + camera_target = home; + +} + +void camera() +{ + //decide what happens to camera depending on camera mode + switch(camera_mode) + { + case 0: + //do nothing, i.e lock camera in place + break; + case 1: + //stabilize + target_vector.x=0; //east to west gives +tive value (i.e. longitude) + target_vector.y=0; //south to north gives +tive value (i.e. latitude) + target_vector.z=100; //downwards is +tive + camera_move(); + break; + case 2: + //track target + if(g_gps->fix) + { + target_vector=get_location_vector(¤t_loc,&camera_target); + camera_move(); + } + break; + } +} + +void camera_move() +{ + Matrix3f m = dcm.get_dcm_transposed(); + Vector3 targ = m*target_vector; //to do: find out notion of x y convention + + switch(gimbal_mode) + { + case 0: // pitch & roll + cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch + cam_roll = degrees(atan2(targ.y, targ.z)); //roll + // check_limits(pitch); + // check_limits(roll); + // camera_out(); + break; + + case 1: // pitch and yaw + cam_tilt = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; + cam_pan = 9000 + atan2(-targ.y, targ.x) * 5729.57795; + if (cam_pan < 0) cam_pan += 36000; + // check_limits(pitch); + // check_limits(yaw); + // camera_out(); + break; + + /* case 2: // pitch, roll & yaw - not started + float cam_ritch = 100; + float cam_yoll = 100; + float cam_paw = 100; + break; */ + } +} + +/* void check_limits(axis,variable) // Use servo definitions to calculate for all servo throws - TO DO +{ + // find limits of servo range in deg + track_pan_right = PAN_CENTER + (PAN_RANGE/2); + track_pan_left = track_pan_right + (360 - PAN_RANGE); + if (track_pan_left > 360){ + track_pan_left = track_pan_left - 360; + } + // check track_bearing is "safe" - not outside pan servo limits + // if the bearing lies in the servo dead zone change bearing to closet edge + if (track_bearing < track_pan_left && track_bearing > track_pan_right){ + track_oor_l = abs(track_bearing - track_pan_left); + track_oor_r = abs(track_bearing - track_pan_right); + if (track_oor_r > track_oor_l){ + track_bearing = track_pan_right; + } + if (track_oor_l > track_oor_r){ + track_bearing = track_pan_left; + } + } + // center bearing to cam_servo center + track_pan_deg = track_bearing - PAN_CENTER; + // make negative is left rotation + if (track_pan_deg > 180){ + track_pan_deg = (180 - (track_pan_deg - 180)) * -1; + } + +} */ + +void camera_out() +{ + switch(gimbal_mode) + { + case 0: // pitch & roll + g.rc_camera_pitch.servo_out = cam_pitch; + g.rc_camera_pitch.calc_pwm(); + g.rc_camera_roll.servo_out = cam_roll; + g.rc_camera_roll.calc_pwm(); + break; + + case 1: // pitch and yaw + g.rc_camera_pitch.servo_out = cam_tilt; + g.rc_camera_pitch.calc_pwm(); + g.rc_camera_roll.servo_out = cam_pan; // borrowing roll servo output for pan/yaw + g.rc_camera_roll.calc_pwm(); + break; + + /*case 2: // pitch, roll & yaw + g.rc_camera_pitch.servo_out = cam_ritch; + g.rc_camera_pitch.calc_pwm(); + + g.rc_camera_roll.servo_out = cam_yoll; + g.rc_camera_roll.calc_pwm(); + + g.rc_camera_yaw.servo_out = cam_paw; // camera_yaw doesn't exist it should unless we use another channel + g.rc_camera_yaw.calc_pwm(); + break; */ + } +#if defined PITCH_SERVO + APM_RC.OutputCh(PITCH_SERVO, g.rc_camera_pitch.radio_out); +#endif +#if defined ROLL_SERVO + APM_RC.OutputCh(ROLL_SERVO, g.rc_camera_roll.radio_out); +#endif +/*#if defined YAW_SERVO + APM_RC.OutputCh(YAW_SERVO, g.rc_camera_yaw.radio_out); +#endif */ + +#if CAM_DEBUG == ENABLED + //for debugging purposes + Serial.println(); + Serial.print("current_loc: lat: "); + Serial.print(current_loc.lat); + Serial.print(", lng: "); + Serial.print(current_loc.lng); + Serial.print(", alt: "); + Serial.print(current_loc.alt); + Serial.println(); + Serial.print("target_loc: lat: "); + Serial.print(camera_target.lat); + Serial.print(", lng: "); + Serial.print(camera_target.lng); + Serial.print(", alt: "); + Serial.print(camera_target.alt); + Serial.print(", distance: "); + Serial.print(get_distance(¤t_loc,&camera_target)); + Serial.print(", bearing: "); + Serial.print(get_bearing(¤t_loc,&camera_target)); + Serial.println(); + Serial.print("dcm_angles: roll: "); + Serial.print(degrees(dcm.roll)); + Serial.print(", pitch: "); + Serial.print(degrees(dcm.pitch)); + Serial.print(", yaw: "); + Serial.print(degrees(dcm.yaw)); + Serial.println(); + Serial.print("target_vector: x: "); + Serial.print(target_vector.x,2); + Serial.print(", y: "); + Serial.print(target_vector.y,2); + Serial.print(", z: "); + Serial.print(target_vector.z,2); + Serial.println(); + Serial.print("rotated_target_vector: x: "); + Serial.print(targ.x,2); + Serial.print(", y: "); + Serial.print(targ.y,2); + Serial.print(", z: "); + Serial.print(targ.z,2); + Serial.println(); + Serial.print("gimbal type 0: roll: "); + Serial.print(roll); + Serial.print(", pitch: "); + Serial.print(pitch); + Serial.println(); + /* Serial.print("gimbal type 1: pitch: "); + Serial.print(pan); + Serial.print(", roll: "); + Serial.print(tilt); + Serial.println(); */ + /* Serial.print("gimbal type 2: pitch: "); + Serial.print(ritch); + Serial.print(", roll: "); + Serial.print(yoll); + Serial.print(", yaw: "); + Serial.print(paw); + Serial.println(); */ +#endif +} +#endif diff --git a/ArduPlane/Cam_trigger.pde b/ArduPlane/Cam_trigger.pde new file mode 100644 index 0000000000..7b03a90c8b --- /dev/null +++ b/ArduPlane/Cam_trigger.pde @@ -0,0 +1,41 @@ +void servo_pic() // Servo operated camera +{ + APM_RC.OutputCh(CAM_SERVO,1500 + (333)); // Camera click, not enough - add more, wring way - put a minus before bracket number (-300) + delay(250); // delay + APM_RC.OutputCh(CAM_SERVO,1500); // Return servo to mid position +} + +void relay_picture() // basic relay activation +{ + relay_on(); + delay(250); // 0.25 seconds delay + relay_off(); +} + +void throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. +{ + g.channel_throttle.radio_out = g.throttle_min; + if (thr_pic = 10){ + servo_pic(); // triggering method + thr_pic = 0; + g.channel_throttle.radio_out = g.throttle_cruise; + } + thr_pic++; +} + +void distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. +{ + g.channel_throttle.radio_out = g.throttle_min; + if (wp_distance < 3){ + servo_pic(); // triggering method + g.channel_throttle.radio_out = g.throttle_cruise; + } +} + +void NPN_trigger() // hacked the circuit to run a transistor? use this trigger to send output. +{ + // To Do: Assign pin spare pin for output + digitalWrite(camtrig, HIGH); + delay(50); + digitalWrite(camtrig, LOW); +} diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index a654d823eb..a80673966e 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -6,7 +6,7 @@ #ifndef __GCS_H #define __GCS_H -#include +#include #include #include #include @@ -40,7 +40,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(FastSerial *port) { _port = port; } + void init(BetterStream *port) { _port = port; } /// Update GCS state. /// @@ -119,7 +119,7 @@ public: protected: /// The stream we are communicating over - FastSerial *_port; + BetterStream *_port; }; // @@ -139,7 +139,7 @@ class GCS_MAVLINK : public GCS_Class public: GCS_MAVLINK(AP_Var::Key key); void update(void); - void init(FastSerial *port); + void init(BetterStream *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const prog_char_t *str); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index db34c15389..b578aedcca 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -4,10 +4,6 @@ #include "Mavlink_Common.h" -// use this to prevent recursion during sensor init -static bool in_mavlink_delay; - - GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : packet_drops(0), @@ -18,21 +14,21 @@ waypoint_receive_timeout(1000), // 1 second // stream rates _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), - // AP_VAR //ref //index, default, name - streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), - streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), - streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), - streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), - streamRatePosition (&_group, 4, 0, PSTR("POSITION")), - streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), - streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), - streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) -{ +streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), +streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), +streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), +streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), +streamRatePosition (&_group, 4, 0, PSTR("POSITION")), +streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), +streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), +streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) + +{ } void -GCS_MAVLINK::init(FastSerial * port) +GCS_MAVLINK::init(BetterStream * port) { GCS_Class::init(port); if (port == &Serial) { // to split hil vs gcs @@ -57,6 +53,8 @@ GCS_MAVLINK::update(void) { uint8_t c = comm_receive_ch(chan); + + // Try to get a new message if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); } @@ -64,6 +62,11 @@ GCS_MAVLINK::update(void) // Update packet drops counter packet_drops += status.packet_rx_drop_count; + + + + + // send out queued params/ waypoints _queued_send(); @@ -84,55 +87,41 @@ void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { + if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax)) + send_message(MSG_RAW_IMU); - if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ - send_message(MSG_RAW_IMU1); - send_message(MSG_RAW_IMU2); - send_message(MSG_RAW_IMU3); - //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); - } - - if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { - send_message(MSG_EXTENDED_STATUS1); - send_message(MSG_EXTENDED_STATUS2); + if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) { + send_message(MSG_EXTENDED_STATUS); send_message(MSG_GPS_STATUS); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); - //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); } - if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { - // sent with GPS read + if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) { send_message(MSG_LOCATION); - //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); } - if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { + if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) { // This is used for HIL. Do not change without discussing with HIL maintainers send_message(MSG_SERVO_OUT); - //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); } - if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { + if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); - //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); } - if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info + if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info send_message(MSG_ATTITUDE); - //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); } - if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info + if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info send_message(MSG_VFR_HUD); - //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); } - if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ + if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){ // Available datastream - //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } @@ -140,7 +129,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) void GCS_MAVLINK::send_message(uint8_t id, uint32_t param) { - mavlink_send_message(chan,id, packet_drops); + mavlink_send_message(chan,id,param,packet_drops); } void @@ -169,9 +158,9 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { - struct Location tell_command = {}; // command for telemetry + struct Location tell_command; // command for telemetry static uint8_t mav_nav=255; // For setting mode (some require receipt of 2 messages...) - + switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: @@ -179,18 +168,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; int freq = 0; // packet frequency - if (packet.start_stop == 0) - freq = 0; // stop sending - else if (packet.start_stop == 1) - freq = packet.req_message_rate; // start sending - else - break; + if (packet.start_stop == 0) freq = 0; // stop sending + else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending + else break; switch(packet.req_stream_id){ @@ -204,7 +188,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) streamRateExtra2 = freq; streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. break; - case MAV_DATA_STREAM_RAW_SENSORS: streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly // we will not continue to broadcast raw sensor data at 50Hz. @@ -212,35 +195,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_DATA_STREAM_EXTENDED_STATUS: streamRateExtendedStatus.set_and_save(freq); break; - case MAV_DATA_STREAM_RC_CHANNELS: streamRateRCChannels.set_and_save(freq); break; - case MAV_DATA_STREAM_RAW_CONTROLLER: streamRateRawController.set_and_save(freq); break; - - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; - + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; case MAV_DATA_STREAM_POSITION: streamRatePosition.set_and_save(freq); break; - case MAV_DATA_STREAM_EXTRA1: streamRateExtra1.set_and_save(freq); break; - case MAV_DATA_STREAM_EXTRA2: streamRateExtra2.set_and_save(freq); break; - case MAV_DATA_STREAM_EXTRA3: streamRateExtra3.set_and_save(freq); break; - default: break; } @@ -253,11 +228,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_action_t packet; mavlink_msg_action_decode(msg, &packet); if (mavlink_check_target(packet.target,packet.target_component)) break; - + uint8_t result = 0; // do action - send_text_P(SEVERITY_LOW,PSTR("action received: ")); + send_text_P(SEVERITY_LOW,PSTR("action received: ")); //Serial.println(packet.action); switch(packet.action){ @@ -357,13 +332,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) default: break; } - + mavlink_msg_action_ack_send( chan, packet.action, result ); - + break; } @@ -372,38 +347,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg, &packet); - + switch(packet.mode){ case MAV_MODE_MANUAL: set_mode(MANUAL); break; - - case MAV_MODE_GUIDED: - set_mode(GUIDED); + + case MAV_MODE_GUIDED: //For future use break; - + case MAV_MODE_AUTO: if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); mav_nav = 255; break; - + case MAV_MODE_TEST1: set_mode(STABILIZE); break; - case MAV_MODE_TEST2: - if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A); - if(mav_nav == 2) set_mode(FLY_BY_WIRE_B); - //if(mav_nav == 3) set_mode(FLY_BY_WIRE_C); - mav_nav = 255; + set_mode(FLY_BY_WIRE_A); + break; + case MAV_MODE_TEST3: + set_mode(FLY_BY_WIRE_B); break; - } } - + case MAVLINK_MSG_ID_SET_NAV_MODE: { // decode @@ -413,7 +385,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mav_nav = packet.nav_mode; break; } - + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); @@ -421,8 +393,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_list_t packet; mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // Start sending waypoints mavlink_msg_waypoint_count_send( @@ -439,7 +410,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); @@ -451,35 +421,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_t packet; mavlink_msg_waypoint_request_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // send waypoint tell_command = get_wp_with_index(packet.seq); // set frame of waypoint uint8_t frame; - - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame } else { frame = MAV_FRAME_GLOBAL; // reference frame } - + float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) - - if (packet.seq == (uint16_t)g.waypoint_index) - current = 1; - + if (packet.seq == g.waypoint_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) - float x = 0, y = 0, z = 0; - if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { + if (tell_command.id < MAV_CMD_NAV_LAST) { // command needs scaling x = tell_command.lat/1.0e7; // local (x), global (latitude) y = tell_command.lng/1.0e7; // local (y), global (longitude) @@ -489,63 +452,50 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) } } + - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_CONDITION_CHANGE_ALT: + case MAV_CMD_DO_SET_HOME: + param1 = tell_command.p1; + break; - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - param1 = tell_command.p1; - break; + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1*10; // APM loiter time is in ten second increments + break; - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1*10; // APM loiter time is in ten second increments - break; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; - case MAV_CMD_CONDITION_CHANGE_ALT: - x=0; // Clear fields loaded above that we don't want sent for this command - y=0; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - param1 = tell_command.p1; - break; + case MAV_CMD_DO_REPEAT_SERVO: + param4 = tell_command.lng; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + param3 = tell_command.lat; + param2 = tell_command.alt; + param1 = tell_command.p1; + break; - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - param3 = tell_command.lat; - param2 = tell_command.alt; - param1 = tell_command.p1; - break; + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + } - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - } - - mavlink_msg_waypoint_send(chan,msg->sysid, - msg->compid, - packet.seq, - frame, - tell_command.id, - current, - autocontinue, - param1, - param2, - param3, - param4, - x, - y, - z); + mavlink_msg_waypoint_send(chan,msg->sysid, + msg->compid,packet.seq,frame,tell_command.id,current,autocontinue, + param1,param2,param3,param4,x,y,z); // update last waypoint comm stamp waypoint_timelast_send = millis(); @@ -561,6 +511,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_ack_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; + // check for error + uint8_t type = packet.type; // ok (0), error(1) + // turn off waypoint send waypoint_sending = false; break; @@ -591,15 +544,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_clear_all_t packet; mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // clear all waypoints uint8_t type = 0; // ok (0), error(1) g.waypoint_total.set_and_save(0); // send acknowledgement 3 times to makes sure it is received - for (int i=0;i<3;i++) - mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type); + for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); break; } @@ -634,7 +586,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) packet.count = MAX_WAYPOINTS; } g.waypoint_total.set_and_save(packet.count - 1); - waypoint_timelast_receive = millis(); waypoint_receiving = true; waypoint_sending = false; @@ -642,58 +593,55 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } -#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS - case MAVLINK_MSG_ID_SET_MAG_OFFSETS: - { - mavlink_set_mag_offsets_t packet; - mavlink_msg_set_mag_offsets_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - compass.set_offsets(Vector3f(packet.mag_ofs_x, packet.mag_ofs_y, packet.mag_ofs_z)); - break; - } -#endif - - // XXX receive a WP from GCS and store in EEPROM case MAVLINK_MSG_ID_WAYPOINT: { + // Check if receiving waypiont + if (!waypoint_receiving) break; + // decode mavlink_waypoint_t packet; mavlink_msg_waypoint_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) break; + + // store waypoint + uint8_t loadAction = 0; // 0 insert in list, 1 exec now + // defaults tell_command.id = packet.command; - switch (packet.frame) - { - case MAV_FRAME_MISSION: - case MAV_FRAME_GLOBAL: - { - tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm - tell_command.options = 0; // absolute altitude - break; - } + switch (packet.frame) + { + case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: + { + tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.options = 0; + break; + } - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; - break; - } - case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - { - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! - break; - } - } + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7*ToDeg(packet.x/ + (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + tell_command.alt = packet.z*1.0e2; + tell_command.options = 1; + break; + } + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + { + tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2; + tell_command.options = 1; + break; + } + } switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields case MAV_CMD_NAV_LOITER_TURNS: @@ -703,7 +651,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.lat = packet.param1; + tell_command.p1 = packet.param1 * 100; break; case MAV_CMD_NAV_LOITER_TIME: @@ -737,53 +685,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission - guided_WP = tell_command; + set_wp_with_index(tell_command, packet.seq); - // add home alt if needed - if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ - guided_WP.alt += home.alt; - } + // update waypoint receiving state machine + waypoint_timelast_receive = millis(); + waypoint_request_i++; - set_mode(GUIDED); + if (waypoint_request_i > g.waypoint_total) + { + uint8_t type = 0; // ok (0), error(1) - // make any new wp uploaded instant (in case we are already in Guided mode) - set_guided_WP(); + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + type); - // verify we recevied the command - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - 0); - - } else { - // Check if receiving waypoints (mission upload expected) - if (!waypoint_receiving) break; - - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) break; - set_wp_with_index(tell_command, packet.seq); - - // update waypoint receiving state machine - waypoint_timelast_receive = millis(); - waypoint_request_i++; - - if (waypoint_request_i > (uint16_t)g.waypoint_total){ - uint8_t type = 0; // ok (0), error(1) - - mavlink_msg_waypoint_ack_send( - chan, - msg->sysid, - msg->compid, - type); - - send_text_P(SEVERITY_LOW,PSTR("flight plan received")); - waypoint_receiving = false; - // XXX ignores waypoint radius for individual waypoints, can - // only set WP_RADIUS parameter - } - } + send_text_P(SEVERITY_LOW,PSTR("flight plan received")); + waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } break; } @@ -795,9 +717,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_param_set_t packet; mavlink_msg_param_set_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system,packet.target_component)) break; // set parameter @@ -814,7 +734,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // add a small amount before casting parameter values // from float to integer to avoid truncating to the // next lower integer value. - float rounding_addition = 0.01; + const float rounding_addition = 0.01; // fetch the variable type ID var_type = vp->meta_type_id(); @@ -827,13 +747,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ((AP_Float16 *)vp)->set_and_save(packet.param_value); } else if (var_type == AP_Var::k_typeid_int32) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int16) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); + } else if (var_type == AP_Var::k_typeid_int8) { - if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); } else { // we don't support mavlink set on this parameter @@ -864,10 +783,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system,packet.target_component)) - break; - + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; @@ -880,20 +797,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) rc_override_fs_timer = millis(); break; } - + case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if(msg->sysid != g.sysid_my_gcs) break; rc_override_fs_timer = millis(); - pmTest1++; break; } - #if HIL_MODE != HIL_MODE_DISABLED +#if HIL_MODE != HIL_MODE_DISABLED // This is used both as a sensor and to pass the location // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: + case MAVLINK_MSG_ID_GPS_RAW: { // decode mavlink_gps_raw_t packet; @@ -1035,7 +951,7 @@ void GCS_MAVLINK::_queued_send() { // Check to see if we are sending parameters - if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) { + if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) { AP_Var *vp; float value; @@ -1068,10 +984,9 @@ GCS_MAVLINK::_queued_send() // request waypoints one by one // XXX note that this is pan-interface if (waypoint_receiving && - (requested_interface == (unsigned)chan) && - waypoint_request_i <= (unsigned)g.waypoint_total && + (requested_interface == chan) && + waypoint_request_i <= g.waypoint_total && mavdelay > 15) { // limits to 3.33 hz - mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, @@ -1082,66 +997,5 @@ GCS_MAVLINK::_queued_send() } } -#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - -static void send_rate(uint16_t low, uint16_t high) { -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - gcs.data_stream_send(low, high); #endif -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(low,high); -#endif -} -/* - a delay() callback that processes MAVLink packets. We set this as the - callback in long running library initialisation routines to allow - MAVLink to process packets while waiting for the initialisation to - complete -*/ -static void mavlink_delay(unsigned long t) -{ - unsigned long tstart; - static unsigned long last_1hz, last_3hz, last_10hz, last_50hz; - - if (in_mavlink_delay) { - // this should never happen, but let's not tempt fate by - // letting the stack grow too much - delay(t); - return; - } - - in_mavlink_delay = true; - - tstart = millis(); - do { - unsigned long tnow = millis(); - if (tnow - last_1hz > 1000) { - last_1hz = tnow; - gcs.send_message(MSG_HEARTBEAT); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.send_message(MSG_HEARTBEAT); -#endif - send_rate(1, 3); - } - if (tnow - last_3hz > 333) { - last_3hz = tnow; - send_rate(3, 5); - } - if (tnow - last_10hz > 100) { - last_10hz = tnow; - send_rate(5, 45); - } - if (tnow - last_50hz > 20) { - last_50hz = tnow; - gcs.update(); -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.update(); -#endif - send_rate(45, 1000); - } - delay(1); - } while (millis() - tstart < t); - - in_mavlink_delay = false; -} diff --git a/ArduPlane/HIL.h b/ArduPlane/HIL.h index 70ad73d6de..85b65fa1be 100644 --- a/ArduPlane/HIL.h +++ b/ArduPlane/HIL.h @@ -8,7 +8,7 @@ # if HIL_MODE != HIL_MODE_DISABLED -#include +#include #include #include #include @@ -41,7 +41,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(FastSerial *port) { _port = port; } + void init(BetterStream *port) { _port = port; } /// Update HIL state. /// @@ -83,7 +83,7 @@ public: protected: /// The stream we are communicating over - FastSerial *_port; + BetterStream *_port; }; // @@ -111,7 +111,7 @@ class HIL_XPLANE : public HIL_Class public: HIL_XPLANE(); void update(void); - void init(FastSerial *port); + void init(BetterStream *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const prog_char_t *str); diff --git a/ArduPlane/HIL_Xplane.pde b/ArduPlane/HIL_Xplane.pde index 4f01c19028..89721b6158 100644 --- a/ArduPlane/HIL_Xplane.pde +++ b/ArduPlane/HIL_Xplane.pde @@ -55,7 +55,7 @@ HIL_XPLANE::HIL_XPLANE() : } void -HIL_XPLANE::init(FastSerial * port) +HIL_XPLANE::init(BetterStream * port) { HIL_Class::init(port); hilPacketDecoder = new AP_GPS_IMU(port); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 59a9c66b0b..0157bd1727 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -1,7 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if LOGGING_ENABLED == ENABLED - // Code to Write and Read packets from DataFlash log memory // Code to interact with the user to dump or erase logs @@ -12,6 +10,7 @@ // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. +static int8_t print_log_menu(uint8_t argc, const Menu::arg *argv); static int8_t dump_log(uint8_t argc, const Menu::arg *argv); static int8_t erase_logs(uint8_t argc, const Menu::arg *argv); static int8_t select_logs(uint8_t argc, const Menu::arg *argv); @@ -28,14 +27,13 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv) " enable |all enable logging or everything\n" " disable |all disable logging or everything\n" "\n")); - return 0; } // Creates a constant array of structs representing menu options // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Coommon for implementation details -static const struct Menu::command log_menu_commands[] PROGMEM = { +const struct Menu::command log_menu_commands[] PROGMEM = { {"dump", dump_log}, {"erase", erase_logs}, {"enable", select_logs}, @@ -46,8 +44,6 @@ static const struct Menu::command log_menu_commands[] PROGMEM = { // A Macro to create the Menu MENU2(log_menu, "Log", log_menu_commands, print_log_menu); -static void get_log_boundaries(byte log_num, int & start_page, int & end_page); - static bool print_log_menu(void) { @@ -84,7 +80,7 @@ print_log_menu(void) Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); for(int i=1;i 0) { for(int i=0;i 7){ logvar = DataFlash.ReadInt(); }else{ @@ -559,7 +550,7 @@ static void Log_Read_Performance() } // Read a command processing packet -static void Log_Read_Cmd() +void Log_Read_Cmd() { byte logvarb; long logvarl; @@ -578,7 +569,7 @@ static void Log_Read_Cmd() Serial.println(" "); } -static void Log_Read_Startup() +void Log_Read_Startup() { byte logbyte = DataFlash.ReadByte(); @@ -593,7 +584,7 @@ static void Log_Read_Startup() } // Read an attitude packet -static void Log_Read_Attitude() +void Log_Read_Attitude() { Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), DataFlash.ReadInt(), @@ -602,22 +593,21 @@ static void Log_Read_Attitude() } // Read a mode packet -static void Log_Read_Mode() +void Log_Read_Mode() { Serial.printf_P(PSTR("MOD:")); Serial.println(flight_mode_strings[DataFlash.ReadByte()]); } // Read a GPS packet -static void Log_Read_GPS() +void Log_Read_GPS() { - Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"), + Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"), DataFlash.ReadLong(), (int)DataFlash.ReadByte(), (int)DataFlash.ReadByte(), (float)DataFlash.ReadLong() / t7, (float)DataFlash.ReadLong() / t7, - (float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing (float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0, (float)DataFlash.ReadLong() / 100.0, @@ -626,7 +616,7 @@ static void Log_Read_GPS() } // Read a raw accel/gyro packet -static void Log_Read_Raw() +void Log_Read_Raw() { float logvar; Serial.printf_P(PSTR("RAW:")); @@ -639,19 +629,13 @@ static void Log_Read_Raw() } // Read the DataFlash log memory : Packet Parser -static void Log_Read(int start_page, int end_page) +void Log_Read(int start_page, int end_page) { byte data; byte log_step = 0; int packet_count = 0; int page = start_page; - #ifdef AIRFRAME_NAME - Serial.printf_P(PSTR((AIRFRAME_NAME) - #endif - Serial.printf_P(PSTR("\n" THISFIRMWARE - "\nFree RAM: %lu\n"), - freeRAM()); DataFlash.StartRead(start_page); while (page < end_page && page != -1){ @@ -728,23 +712,4 @@ static void Log_Read(int start_page, int end_page) Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); } -#else // LOGGING_ENABLED -// dummy functions -static void Log_Write_Mode(byte mode) {} -static void Log_Write_Startup(byte type) {} -static void Log_Write_Cmd(byte num, struct Location *wp) {} -static void Log_Write_Current() {} -static void Log_Write_Nav_Tuning() {} -static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt, - long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {} -static void Log_Write_Performance() {} -static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -static byte get_num_logs(void) { return 0; } -static void start_new_log(byte num_existing_logs) {} -static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {} -static void Log_Write_Control_Tuning() {} -static void Log_Write_Raw() {} - - -#endif // LOGGING_ENABLED diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..6c701c47df 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -25,36 +25,21 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } -// try to send a message, return false if it won't fit in the serial tx buffer -static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) + +void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) { uint64_t timeStamp = micros(); - int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; - -#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false - - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // defer any messages on the telemetry port for 1 second after - // bootup, to try to prevent bricking of Xbees - return false; - } - switch(id) { case MSG_HEARTBEAT: - { - CHECK_PAYLOAD_SIZE(HEARTBEAT); mavlink_msg_heartbeat_send( chan, mavlink_system.type, MAV_AUTOPILOT_ARDUPILOTMEGA); break; - } - case MSG_EXTENDED_STATUS1: + case MSG_EXTENDED_STATUS: { - CHECK_PAYLOAD_SIZE(SYS_STATUS); - uint8_t mode = MAV_MODE_UNINIT; uint8_t nav_mode = MAV_NAV_VECTOR; @@ -67,14 +52,9 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; case FLY_BY_WIRE_A: mode = MAV_MODE_TEST2; - nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. break; case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST2; - nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case GUIDED: - mode = MAV_MODE_GUIDED; + mode = MAV_MODE_TEST3; break; case AUTO: mode = MAV_MODE_AUTO; @@ -88,10 +68,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ mode = MAV_MODE_AUTO; nav_mode = MAV_NAV_LOITER; break; - case INITIALISING: - mode = MAV_MODE_UNINIT; - nav_mode = MAV_NAV_GROUNDED; - break; } uint8_t status = MAV_STATE_ACTIVE; @@ -106,20 +82,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ battery_voltage * 1000, battery_remaining, packet_drops); - break; - } - - case MSG_EXTENDED_STATUS2: - { - CHECK_PAYLOAD_SIZE(MEMINFO); - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); break; - } + } case MSG_ATTITUDE: { - CHECK_PAYLOAD_SIZE(ATTITUDE); Vector3f omega = dcm.get_gyro(); mavlink_msg_attitude_send( chan, @@ -135,7 +102,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_LOCATION: { - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_global_position_int_send( chan, @@ -151,7 +117,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_NAV_CONTROLLER_OUTPUT: { if (control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); mavlink_msg_nav_controller_output_send( chan, nav_roll / 1.0e2, @@ -168,7 +133,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_LOCAL_LOCATION: { - CHECK_PAYLOAD_SIZE(LOCAL_POSITION); Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_local_position_send( chan, @@ -184,7 +148,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_GPS_RAW: { - CHECK_PAYLOAD_SIZE(GPS_RAW); mavlink_msg_gps_raw_send( chan, timeStamp, @@ -201,7 +164,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_SERVO_OUT: { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); uint8_t rssi = 1; // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with HIL maintainers @@ -221,7 +183,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_RADIO_IN: { - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); uint8_t rssi = 1; mavlink_msg_rc_channels_raw_send( chan, @@ -239,7 +200,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_RADIO_OUT: { - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); mavlink_msg_servo_output_raw_send( chan, g.channel_roll.radio_out, @@ -255,7 +215,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_VFR_HUD: { - CHECK_PAYLOAD_SIZE(VFR_HUD); mavlink_msg_vfr_hud_send( chan, (float)airspeed / 100.0, @@ -268,12 +227,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } #if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU1: + case MSG_RAW_IMU: { - CHECK_PAYLOAD_SIZE(RAW_IMU); Vector3f accel = imu.get_accel(); Vector3f gyro = imu.get_gyro(); - //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); mavlink_msg_raw_imu_send( @@ -288,42 +245,21 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ compass.mag_x, compass.mag_y, compass.mag_z); - break; - } - case MSG_RAW_IMU2: - { - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + /* This message is not working. Why? mavlink_msg_scaled_pressure_send( - chan, - timeStamp, - (float)barometer.Press/100.0, - (float)(barometer.Press-g.ground_pressure)/100.0, - (int)(barometer.Temp*10)); - break; - } - - case MSG_RAW_IMU3: - { - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - Vector3f mag_offsets = compass.get_offsets(); - - mavlink_msg_sensor_offsets_send(chan, - mag_offsets.x, - mag_offsets.y, - mag_offsets.z, - compass.get_declination(), - barometer.RawPress, - barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), - imu.ax(), imu.ay(), imu.az()); + chan, + timeStamp, + (float)barometer.Press/100., + (float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw + (int)(barometer.Temp*100)); + */ break; } #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE case MSG_GPS_STATUS: { - CHECK_PAYLOAD_SIZE(GPS_STATUS); mavlink_msg_gps_status_send( chan, g_gps->num_sats, @@ -337,7 +273,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ case MSG_CURRENT_WAYPOINT: { - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); mavlink_msg_waypoint_current_send( chan, g.waypoint_index); @@ -347,77 +282,10 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ default: break; } - return true; -} - - -#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of - // switch types in mavlink_try_send_message() -static struct mavlink_queue { - uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; - uint8_t next_deferred_message; - uint8_t num_deferred_messages; -} mavlink_queue[2]; - -// send a message using mavlink -static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) -{ - uint8_t i, nextid; - struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; - - // see if we can send the deferred messages, if any - while (q->num_deferred_messages != 0) { - if (!mavlink_try_send_message(chan, - q->deferred_messages[q->next_deferred_message], - packet_drops)) { - break; - } - q->next_deferred_message++; - if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { - q->next_deferred_message = 0; - } - q->num_deferred_messages--; - } - - if (id == MSG_RETRY_DEFERRED) { - return; - } - - // this message id might already be deferred - for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { - if (q->deferred_messages[nextid] == id) { - // its already deferred, discard - return; - } - nextid++; - if (nextid == MAX_DEFERRED_MESSAGES) { - nextid = 0; - } - } - - if (q->num_deferred_messages != 0 || - !mavlink_try_send_message(chan, id, packet_drops)) { - // can't send it now, so defer it - if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { - // the defer buffer is full, discard - return; - } - nextid = q->next_deferred_message + q->num_deferred_messages; - if (nextid >= MAX_DEFERRED_MESSAGES) { - nextid -= MAX_DEFERRED_MESSAGES; - } - q->deferred_messages[nextid] = id; - q->num_deferred_messages++; - } } void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str) { - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // don't send status MAVLink messages for 1 second after - // bootup, to try to prevent Xbee bricking - return; - } mavlink_msg_statustext_send( chan, severity, diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 570c38800a..8d8001b124 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -1,465 +1,381 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef PARAMETERS_H -#define PARAMETERS_H - -#include - -// Global parameter class. -// -class Parameters { -public: - // The version of the layout as described by the parameter enum. - // - // When changing the parameter enum in an incompatible fashion, this - // value should be incremented by one. - // - // The increment will prevent old parameters from being used incorrectly - // by newer code. - // - static const uint16_t k_format_version = 11; - - // The parameter software_type is set up solely for ground station use - // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) - // GCS will interpret values 0-9 as ArduPilotMega. Developers may use - // values within that range to identify different branches. - // - static const uint16_t k_software_type = 0; // 0 for APM trunk - - // - // Parameter identities. - // - // The enumeration defined here is used to ensure that every parameter - // or parameter group has a unique ID number. This number is used by - // AP_Var to store and locate parameters in EEPROM. - // - // Note that entries without a number are assigned the next number after - // the entry preceding them. When adding new entries, ensure that they - // don't overlap. - // - // Try to group related variables together, and assign them a set - // range in the enumeration. Place these groups in numerical order - // at the end of the enumeration. - // - // WARNING: Care should be taken when editing this enumeration as the - // AP_Var load/save code depends on the values here to identify - // variables saved in EEPROM. - // - // - enum { - // Layout version number, always key zero. - // - k_param_format_version = 0, - k_param_software_type, - - - // Misc - // - - k_param_auto_trim, - k_param_switch_enable, - k_param_log_bitmask, - k_param_pitch_trim, - k_param_mix_mode, - k_param_reverse_elevons, - k_param_reverse_ch1_elevon, - k_param_reverse_ch2_elevon, - k_param_flap_1_percent, - k_param_flap_1_speed, - k_param_flap_2_percent, - k_param_flap_2_speed, - k_param_num_resets, - - - // 110: Telemetry control - // - k_param_streamrates_port0 = 110, - k_param_streamrates_port3, - k_param_sysid_this_mav, - k_param_sysid_my_gcs, - k_param_serial3_baud, - - // 120: Fly-by-wire control - // - k_param_flybywire_airspeed_min = 120, - k_param_flybywire_airspeed_max, - - // - // 130: Sensor parameters - // - k_param_IMU_calibration = 130, - k_param_altitude_mix, - k_param_airspeed_ratio, - k_param_ground_temperature, - k_param_ground_pressure, - k_param_compass_enabled, - k_param_compass, - k_param_battery_monitoring, - k_param_pack_capacity, - k_param_airspeed_offset, - k_param_sonar_enabled, - k_param_airspeed_enabled, - - // - // 150: Navigation parameters - // - k_param_crosstrack_gain = 150, - k_param_crosstrack_entry_angle, - k_param_roll_limit, - k_param_pitch_limit_max, - k_param_pitch_limit_min, - k_param_airspeed_cruise, - k_param_RTL_altitude, - k_param_inverted_flight_ch, - - // - // 170: Radio settings - // - k_param_channel_roll = 170, - k_param_channel_pitch, - k_param_channel_throttle, - k_param_channel_yaw, - k_param_rc_5, - k_param_rc_6, - k_param_rc_7, - k_param_rc_8, - - k_param_throttle_min, - k_param_throttle_max, - k_param_throttle_fs_enabled, - k_param_throttle_fs_value, - k_param_throttle_cruise, - - k_param_short_fs_action, - k_param_long_fs_action, - k_param_gcs_heartbeat_fs_enabled, - k_param_throttle_slewrate, - - k_param_rc_5_funct, - k_param_rc_6_funct, - k_param_rc_7_funct, - k_param_rc_8_funct, - - // - // 200: Feed-forward gains - // - k_param_kff_pitch_compensation = 200, - k_param_kff_rudder_mix, - k_param_kff_pitch_to_throttle, - k_param_kff_throttle_to_pitch, - - // - // 210: flight modes - // - k_param_flight_mode_channel = 210, - k_param_flight_mode1, - k_param_flight_mode2, - k_param_flight_mode3, - k_param_flight_mode4, - k_param_flight_mode5, - k_param_flight_mode6, - - // - // 220: Waypoint data - // - k_param_waypoint_mode = 220, - k_param_waypoint_total, - k_param_waypoint_index, - k_param_waypoint_radius, - k_param_loiter_radius, - - // - // 240: PID Controllers - // - // Heading-to-roll PID: - // heading error from command to roll command deviation from trim - // (bank to turn strategy) - // - k_param_heading_to_roll_PID = 240, - - // Roll-to-servo PID: - // roll error from command to roll servo deviation from trim - // (tracks commanded bank angle) - // - k_param_roll_to_servo_PID, - - // - // Pitch control - // - // Pitch-to-servo PID: - // pitch error from command to pitch servo deviation from trim - // (front-side strategy) - // - k_param_pitch_to_servo_PID, - - // Airspeed-to-pitch PID: - // airspeed error from command to pitch servo deviation from trim - // (back-side strategy) - // - k_param_airspeed_to_pitch_PID, - - // - // Yaw control - // - // Yaw-to-servo PID: - // yaw rate error from command to yaw servo deviation from trim - // (stabilizes dutch roll) - // - k_param_yaw_to_servo_PID, - - // - // Throttle control - // - // Energy-to-throttle PID: - // total energy error from command to throttle servo deviation from trim - // (throttle back-side strategy alternative) - // - k_param_energy_to_throttle_PID, - - // Altitude-to-pitch PID: - // altitude error from command to pitch servo deviation from trim - // (throttle front-side strategy alternative) - // - k_param_altitude_to_pitch_PID, - - // 254,255: reserved - }; - - AP_Int16 format_version; - AP_Int8 software_type; - - // Telemetry control - // - AP_Int16 sysid_this_mav; - AP_Int16 sysid_my_gcs; - AP_Int8 serial3_baud; - - // Feed-forward gains - // - AP_Float kff_pitch_compensation; - AP_Float kff_rudder_mix; - AP_Float kff_pitch_to_throttle; - AP_Float kff_throttle_to_pitch; - - // Crosstrack navigation - // - AP_Float crosstrack_gain; - AP_Int16 crosstrack_entry_angle; - - // Estimation - // - AP_Float altitude_mix; - AP_Float airspeed_ratio; - AP_Int16 airspeed_offset; - - // Waypoints - // - AP_Int8 waypoint_mode; - AP_Int8 waypoint_total; - AP_Int8 waypoint_index; - AP_Int8 waypoint_radius; - AP_Int8 loiter_radius; - - // Fly-by-wire - // - AP_Int8 flybywire_airspeed_min; - AP_Int8 flybywire_airspeed_max; - - // Throttle - // - AP_Int8 throttle_min; - AP_Int8 throttle_max; - AP_Int8 throttle_slewrate; - AP_Int8 throttle_fs_enabled; - AP_Int16 throttle_fs_value; - AP_Int8 throttle_cruise; - - // Failsafe - AP_Int8 short_fs_action; - AP_Int8 long_fs_action; - AP_Int8 gcs_heartbeat_fs_enabled; - - // Flight modes - // - AP_Int8 flight_mode_channel; - AP_Int8 flight_mode1; - AP_Int8 flight_mode2; - AP_Int8 flight_mode3; - AP_Int8 flight_mode4; - AP_Int8 flight_mode5; - AP_Int8 flight_mode6; - - // Navigational maneuvering limits - // - AP_Int16 roll_limit; - AP_Int16 pitch_limit_max; - AP_Int16 pitch_limit_min; - - // Misc - // - AP_Int8 auto_trim; - AP_Int8 switch_enable; - AP_Int8 mix_mode; - AP_Int8 reverse_elevons; - AP_Int8 reverse_ch1_elevon; - AP_Int8 reverse_ch2_elevon; - AP_Int16 num_resets; - AP_Int16 log_bitmask; - AP_Int16 airspeed_cruise; - AP_Int16 pitch_trim; - AP_Int16 RTL_altitude; - AP_Int16 ground_temperature; - AP_Int32 ground_pressure; - AP_Int8 compass_enabled; - AP_Int16 angle_of_attack; - AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current - AP_Int16 pack_capacity; // Battery pack capacity less reserve - AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger - AP_Int8 sonar_enabled; - AP_Int8 airspeed_enabled; - AP_Int8 flap_1_percent; - AP_Int8 flap_1_speed; - AP_Int8 flap_2_percent; - AP_Int8 flap_2_speed; - - // RC channels - RC_Channel channel_roll; - RC_Channel channel_pitch; - RC_Channel channel_throttle; - RC_Channel channel_rudder; - RC_Channel rc_5; - RC_Channel rc_6; - RC_Channel rc_7; - RC_Channel rc_8; - AP_Int8 rc_5_funct; - AP_Int8 rc_6_funct; - AP_Int8 rc_7_funct; - AP_Int8 rc_8_funct; - - // PID controllers - // - PID pidNavRoll; - PID pidServoRoll; - PID pidServoPitch; - PID pidNavPitchAirspeed; - PID pidServoRudder; - PID pidTeThrottle; - PID pidNavPitchAltitude; - - uint8_t junk; - - // Note: keep initializers here in the same order as they are declared above. - Parameters() : - // variable default key name - //------------------------------------------------------------------------------------------------------- - format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")), - software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")), - - sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), - sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), - serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")), - - kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")), - kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")), - kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")), - kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")), - - crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), - crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), - - altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")), - airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")), - airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")), - - /* XXX waypoint_mode missing here */ - waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), - waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), - waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), - loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), - - flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")), - flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")), - - throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), - throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), - throttle_slewrate (THROTTLE_SLEW_LIMIT, k_param_throttle_slewrate, PSTR("THR_SLEWRATE")), - throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), - throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), - throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), - - short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), - long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), - gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), - - flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")), - flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")), - flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")), - flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")), - flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")), - flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")), - flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")), - - roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")), - pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")), - pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")), - - auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")), - switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")), - mix_mode (ELEVON_MIXING, k_param_mix_mode, PSTR("ELEVON_MIXING")), - reverse_elevons (ELEVON_REVERSE, k_param_reverse_elevons, PSTR("ELEVON_REVERSE")), - reverse_ch1_elevon (ELEVON_CH1_REVERSE, k_param_reverse_ch1_elevon, PSTR("ELEVON_CH1_REV")), - reverse_ch2_elevon (ELEVON_CH2_REVERSE, k_param_reverse_ch2_elevon, PSTR("ELEVON_CH2_REV")), - num_resets (0, k_param_num_resets, PSTR("SYS_NUM_RESETS")), - log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), - airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), - pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), - RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), - ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), - ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), - compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), - flap_1_percent (FLAP_1_PERCENT, k_param_flap_1_percent, PSTR("FLAP_1_PERCNT")), - flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")), - flap_2_percent (FLAP_2_PERCENT, k_param_flap_2_percent, PSTR("FLAP_2_PERCNT")), - flap_2_speed (FLAP_2_SPEED, k_param_flap_2_speed, PSTR("FLAP_2_SPEED")), - - - battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), - pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), - inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")), - sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")), - airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")), - rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")), - rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")), - rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")), - rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")), - - // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! - - // RC channel group key name - //---------------------------------------------------------------------- - channel_roll (k_param_channel_roll, PSTR("RC1_")), - channel_pitch (k_param_channel_pitch, PSTR("RC2_")), - channel_throttle (k_param_channel_throttle, PSTR("RC3_")), - channel_rudder (k_param_channel_yaw, PSTR("RC4_")), - rc_5 (k_param_rc_5, PSTR("RC5_")), - rc_6 (k_param_rc_6, PSTR("RC6_")), - rc_7 (k_param_rc_7, PSTR("RC7_")), - rc_8 (k_param_rc_8, PSTR("RC8_")), - - // PID controller group key name initial P initial I initial D initial imax - //--------------------------------------------------------------------------------------------------------------------------------------- - pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), - pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), - pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), - pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSP2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), - pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), - pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), - pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), - - - junk(0) // XXX just so that we can add things without worrying about the trailing comma - { - } -}; - +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#include + +// Global parameter class. +// +class Parameters { +public: + // The version of the layout as described by the parameter enum. + // + // When changing the parameter enum in an incompatible fashion, this + // value should be incremented by one. + // + // The increment will prevent old parameters from being used incorrectly + // by newer code. + // + static const uint16_t k_format_version = 6; + + // + // Parameter identities. + // + // The enumeration defined here is used to ensure that every parameter + // or parameter group has a unique ID number. This number is used by + // AP_Var to store and locate parameters in EEPROM. + // + // Note that entries without a number are assigned the next number after + // the entry preceding them. When adding new entries, ensure that they + // don't overlap. + // + // Try to group related variables together, and assign them a set + // range in the enumeration. Place these groups in numerical order + // at the end of the enumeration. + // + // WARNING: Care should be taken when editing this enumeration as the + // AP_Var load/save code depends on the values here to identify + // variables saved in EEPROM. + // + // + enum { + // Layout version number, always key zero. + // + k_param_format_version = 0, + + + // Misc + // + + k_param_auto_trim, + k_param_switch_enable, + k_param_log_bitmask, + k_param_pitch_trim, + + // 110: Telemetry control + // + k_param_streamrates_port0 = 110, + k_param_streamrates_port3, + k_param_sysid_this_mav, + k_param_sysid_my_gcs, + + // 120: Fly-by-wire control + // + k_param_flybywire_airspeed_min = 120, + k_param_flybywire_airspeed_max, + + // + // 140: Sensor parameters + // + k_param_IMU_calibration = 140, + k_param_altitude_mix, + k_param_airspeed_ratio, + k_param_ground_temperature, + k_param_ground_pressure, + k_param_compass_enabled, + k_param_compass, + k_param_battery_monitoring, + k_param_pack_capacity, + + // + // 160: Navigation parameters + // + k_param_crosstrack_gain = 160, + k_param_crosstrack_entry_angle, + k_param_roll_limit, + k_param_pitch_limit_max, + k_param_pitch_limit_min, + k_param_airspeed_cruise, + k_param_RTL_altitude, + + // + // 180: Radio settings + // + k_param_channel_roll = 180, + k_param_channel_pitch, + k_param_channel_throttle, + k_param_channel_yaw, + k_param_rc_5, + k_param_rc_6, + k_param_rc_7, + k_param_rc_8, + k_param_rc_9, + k_param_rc_10, + + k_param_throttle_min, + k_param_throttle_max, + k_param_throttle_fs_enabled, + k_param_throttle_fs_value, + k_param_throttle_cruise, + k_param_flight_mode_channel, + k_param_flight_modes, + + k_param_short_fs_action, + k_param_long_fs_action, + k_param_gcs_heartbeat_fs_enabled, + + // + // 200: Feed-forward gains + // + k_param_kff_pitch_compensation = 200, + k_param_kff_rudder_mix, + k_param_kff_pitch_to_throttle, + k_param_kff_throttle_to_pitch, + + // + // 220: Waypoint data + // + k_param_waypoint_mode = 220, + k_param_waypoint_total, + k_param_waypoint_index, + k_param_waypoint_radius, + k_param_loiter_radius, + + // + // 240: PID Controllers + // + // Heading-to-roll PID: + // heading error from commnd to roll command deviation from trim + // (bank to turn strategy) + // + k_param_heading_to_roll_PID = 240, + + // Roll-to-servo PID: + // roll error from command to roll servo deviation from trim + // (tracks commanded bank angle) + // + k_param_roll_to_servo_PID, + + // + // Pitch control + // + // Pitch-to-servo PID: + // pitch error from command to pitch servo deviation from trim + // (front-side strategy) + // + k_param_pitch_to_servo_PID, + + // Airspeed-to-pitch PID: + // airspeed error from commnd to pitch servo deviation from trim + // (back-side strategy) + // + k_param_airspeed_to_pitch_PID, + + // + // Yaw control + // + // Yaw-to-servo PID: + // yaw rate error from commnd to yaw servo deviation from trim + // (stabilizes dutch roll) + // + k_param_yaw_to_servo_PID, + + // + // Throttle control + // + // Energy-to-throttle PID: + // total energy error from command to throttle servo deviation from trim + // (throttle back-side strategy alternative) + // + k_param_energy_to_throttle_PID, + + // Altitude-to-pitch PID: + // altitude error from commnd to pitch servo deviation from trim + // (throttle front-side strategy alternative) + // + k_param_altitude_to_pitch_PID, + + // 255: reserved + }; + + AP_Int16 format_version; + + // Telemetry control + // + AP_Int16 sysid_this_mav; + AP_Int16 sysid_my_gcs; + + // Feed-forward gains + // + AP_Float kff_pitch_compensation; + AP_Float kff_rudder_mix; + AP_Float kff_pitch_to_throttle; + AP_Float kff_throttle_to_pitch; + + // Crosstrack navigation + // + AP_Float crosstrack_gain; + AP_Int16 crosstrack_entry_angle; + + // Estimation + // + AP_Float altitude_mix; + AP_Float airspeed_ratio; + + // Waypoints + // + AP_Int8 waypoint_mode; + AP_Int8 waypoint_total; + AP_Int8 waypoint_index; + AP_Int8 waypoint_radius; + AP_Int8 loiter_radius; + + // Fly-by-wire + // + AP_Int8 flybywire_airspeed_min; + AP_Int8 flybywire_airspeed_max; + + // Throttle + // + AP_Int8 throttle_min; + AP_Int8 throttle_max; + AP_Int8 throttle_fs_enabled; + AP_Int16 throttle_fs_value; + AP_Int8 throttle_cruise; + + // Failsafe + AP_Int8 short_fs_action; + AP_Int8 long_fs_action; + AP_Int8 gcs_heartbeat_fs_enabled; + + // Flight modes + // + AP_Int8 flight_mode_channel; + AP_VarA flight_modes; + + // Navigational maneuvering limits + // + AP_Int16 roll_limit; + AP_Int16 pitch_limit_max; + AP_Int16 pitch_limit_min; + + // Misc + // + AP_Int8 auto_trim; + AP_Int8 switch_enable; + AP_Int16 log_bitmask; + AP_Int16 airspeed_cruise; + AP_Int16 pitch_trim; + AP_Int16 RTL_altitude; + AP_Int16 ground_temperature; + AP_Int32 ground_pressure; + AP_Int8 compass_enabled; + AP_Int16 angle_of_attack; + AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current + AP_Int16 pack_capacity; // Battery pack capacity less reserve + + // RC channels + RC_Channel channel_roll; + RC_Channel channel_pitch; + RC_Channel channel_throttle; + RC_Channel channel_rudder; + RC_Channel rc_5; + RC_Channel rc_6; + RC_Channel rc_7; + RC_Channel rc_8; + RC_Channel rc_camera_pitch; + RC_Channel rc_camera_roll; + + // PID controllers + // + PID pidNavRoll; + PID pidServoRoll; + PID pidServoPitch; + PID pidNavPitchAirspeed; + PID pidServoRudder; + PID pidTeThrottle; + PID pidNavPitchAltitude; + + uint8_t junk; + + // Note: keep initializers here in the same order as they are declared above. + Parameters() : + // variable default key name + //------------------------------------------------------------------------------------------------------- + format_version (k_format_version, k_param_format_version, NULL), + + sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), + sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), + + kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")), + kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")), + kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")), + kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")), + + crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), + crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), + + altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")), + airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")), + + /* XXX waypoint_mode missing here */ + waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), + waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), + loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), + + flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")), + flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")), + + throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), + throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), + throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), + throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), + throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), + + short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")), + long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")), + gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")), + + flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")), + flight_modes (k_param_flight_modes, PSTR("FLTMODE")), + + roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")), + pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")), + pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")), + + auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")), + switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")), + log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")), + airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), + pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), + RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), + ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), + ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), + compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), + battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), + pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), + + // Note - total parameter name length must be less than 14 characters for MAVLink compatibility! + + // RC channel group key name + //---------------------------------------------------------------------- + channel_roll (k_param_channel_roll, PSTR("RC1_")), + channel_pitch (k_param_channel_pitch, PSTR("RC2_")), + channel_throttle (k_param_channel_throttle, PSTR("RC3_")), + channel_rudder (k_param_channel_yaw, PSTR("RC4_")), + rc_5 (k_param_rc_5, PSTR("RC5_")), + rc_6 (k_param_rc_6, PSTR("RC6_")), + rc_7 (k_param_rc_7, PSTR("RC7_")), + rc_8 (k_param_rc_8, PSTR("RC8_")), + rc_camera_pitch (k_param_rc_9, NULL), + rc_camera_roll (k_param_rc_10, NULL), + + // PID controller group key name initial P initial I initial D initial imax + //--------------------------------------------------------------------------------------------------------------------------------------- + pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE), + pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE), + pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE), + pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSPD2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), + pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), + pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), + pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), + + + junk(0) // XXX just so that we can add things without worrying about the trailing comma + { + } +}; + #endif // PARAMETERS_H diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde new file mode 100644 index 0000000000..733a81a129 --- /dev/null +++ b/ArduPlane/WP_activity.pde @@ -0,0 +1,49 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +void waypoint_check() +{ + if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want + if(wp_distance < 10){ // Get as close as it can for you + servo_pic(); + } // DO SOMETHIMNG + } + if(g.waypoint_index == 20){ // When here do whats underneath + servo_pic(); + } +} + +void picture_time_check() +{ + if (picture_time == 1){ + if (wp_distance < 10){ + servo_pic(); // or any camera activation command + } + } +} + +void egg_waypoint() +{ + float temp = (float)(current_loc.alt - home.alt) * .01; + float egg_dist = sqrt(temp / 4.903) * (float)g_gps->ground_speed *.01; + + if(g.waypoint_index == 3){ + if(wp_distance < egg_dist){ + APM_RC.OutputCh(CH_RUDDER,1500 + (-45*10.31)); + } + }else{ + APM_RC.OutputCh(CH_RUDDER,1500 + (45*10.31)); + } +} + +void johann_check() // if you aren't Johann it doesn't really matter :D +{ + APM_RC.OutputCh(CH_7,1500 + (350)); + if(g.waypoint_index > 1 && g.waypoint_index <=18){ // Between these waypoints it will do what you want + if(wp_distance < 10){ // Get as close as it can for you + servo_pic(); + } + } + if(g.waypoint_index == 20){ // When here do whats underneath + servo_pic(); + } +} diff --git a/ArduPlane/climb_rate.pde b/ArduPlane/climb_rate.pde index b7e6b81d74..ec1247a85e 100644 --- a/ArduPlane/climb_rate.pde +++ b/ArduPlane/climb_rate.pde @@ -1,12 +1,10 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - struct DataPoint { unsigned long x; long y; }; DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from -unsigned char hindex; // Index in history for the current data point +unsigned char index; // Index in history for the current data point unsigned long xoffset; unsigned char n; @@ -17,9 +15,12 @@ long yi; long xiyi; unsigned long xi2; -#if 0 // currently unused -static void add_altitude_data(unsigned long xl, long y) + +void add_altitude_data(unsigned long xl, long y) { + unsigned char i; + int dx; + //Reset the regression if our X variable overflowed if (xl < xoffset) n = 0; @@ -29,10 +30,10 @@ static void add_altitude_data(unsigned long xl, long y) n = 0; if (n == ALTITUDE_HISTORY_LENGTH) { - xi -= history[hindex].x; - yi -= history[hindex].y; - xiyi -= (long)history[hindex].x * history[hindex].y; - xi2 -= history[hindex].x * history[hindex].x; + xi -= history[index].x; + yi -= history[index].y; + xiyi -= (long)history[index].x * history[index].y; + xi2 -= history[index].x * history[index].x; } else { if (n == 0) { xoffset = xl; @@ -44,33 +45,31 @@ static void add_altitude_data(unsigned long xl, long y) n++; } - history[hindex].x = xl - xoffset; - history[hindex].y = y; + history[index].x = xl - xoffset; + history[index].y = y; - xi += history[hindex].x; - yi += history[hindex].y; - xiyi += (long)history[hindex].x * history[hindex].y; - xi2 += history[hindex].x * history[hindex].x; + xi += history[index].x; + yi += history[index].y; + xiyi += (long)history[index].x * history[index].y; + xi2 += history[index].x * history[index].x; - if (++hindex >= ALTITUDE_HISTORY_LENGTH) - hindex = 0; + if (++index >= ALTITUDE_HISTORY_LENGTH) + index = 0; } -#endif -#if 0 // unused -static void recalc_climb_rate() +void recalc_climb_rate() { float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2); climb_rate = (int)(slope*100); } -static void print_climb_debug_info() +void print_climb_debug_info() { unsigned char i, j; recalc_climb_rate(); //SendDebugln_P("Climb rate:"); for (i=0; i= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH; //SendDebug_P(" "); //SendDebug(j,DEC); @@ -91,4 +90,3 @@ static void print_climb_debug_info() //SendDebug((float)climb_rate/100,2); //SendDebugln_P(" m/s"); } -#endif diff --git a/ArduPlane/command description.txt b/ArduPlane/command description.txt index 2d3102c9d9..b2668a8b90 100644 --- a/ArduPlane/command description.txt +++ b/ArduPlane/command description.txt @@ -45,7 +45,7 @@ May Commands - these commands are optional to finish Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) - -0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT - alt (finish) rate (cm/sec) - +0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - - Note: rate must be > 10 cm/sec due to integer math MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 0ad04ee3da..5ccbf7e486 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -1,6 +1,6 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void init_commands() +void init_commands() { //read_EEPROM_waypoint_info(); g.waypoint_index.set_and_save(0); @@ -9,19 +9,15 @@ static void init_commands() next_command.id = CMD_BLANK; } -static void update_auto() +void update_auto() { - if (g.waypoint_index >= g.waypoint_total) { - handle_no_commands(); - if(g.waypoint_total == 0) { - next_WP.lat = home.lat + 1000; // so we don't have bad calcs - next_WP.lng = home.lng + 1000; // so we don't have bad calcs - } + if (g.waypoint_index == g.waypoint_total) { + do_RTL(); } } // this is only used by an air-start -static void reload_commands_airstart() +void reload_commands_airstart() { init_commands(); g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? @@ -30,7 +26,7 @@ static void reload_commands_airstart() // Getters // ------- -static struct Location get_wp_with_index(int i) +struct Location get_wp_with_index(int i) { struct Location temp; long mem; @@ -70,13 +66,12 @@ static struct Location get_wp_with_index(int i) // Setters // ------- -static void set_wp_with_index(struct Location temp, int i) +void set_wp_with_index(struct Location temp, int i) { i = constrain(i, 0, g.waypoint_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - - // Set altitude options bitmask - // XXX What is this trying to do? + + // Set altitude options bitmask if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){ temp.options = MASK_OPTIONS_RELATIVE_ALT; } else { @@ -101,26 +96,26 @@ static void set_wp_with_index(struct Location temp, int i) eeprom_write_dword((uint32_t *) mem, temp.lng); } -static void increment_WP_index() +void increment_WP_index() { - if (g.waypoint_index <= g.waypoint_total) { + if (g.waypoint_index < g.waypoint_total) { g.waypoint_index.set_and_save(g.waypoint_index + 1); - SendDebug_P("MSG - WP index is incremented to "); + SendDebug_P("MSG WP index is incremented to "); }else{ //SendDebug_P("MSG Failed to increment WP index of "); // This message is used excessively at the end of a mission } - //SendDebugln(g.waypoint_index,DEC); + SendDebugln(g.waypoint_index,DEC); } -static void decrement_WP_index() +void decrement_WP_index() { if (g.waypoint_index > 0) { g.waypoint_index.set_and_save(g.waypoint_index - 1); } } -static long read_alt_to_hold() +long read_alt_to_hold() { if(g.RTL_altitude < 0) return current_loc.alt; @@ -129,14 +124,30 @@ static long read_alt_to_hold() } +//******************************************************************************** +//This function sets the waypoint and modes for Return to Launch +//******************************************************************************** + +Location get_LOITER_home_wp() +{ + //so we know where we are navigating from + next_WP = current_loc; + + // read home position + struct Location temp = get_wp_with_index(0); + temp.id = MAV_CMD_NAV_LOITER_UNLIM; + temp.alt = read_alt_to_hold(); + return temp; +} + /* This function stores waypoint commands It looks to see what the next command type is and finds the last command. */ -static void set_next_WP(struct Location *wp) +void set_next_WP(struct Location *wp) { //gcs.send_text_P(SEVERITY_LOW,PSTR("load WP")); - SendDebug_P("MSG - wp_index: "); + SendDebug_P("MSG wp_index: "); SendDebugln(g.waypoint_index, DEC); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -162,44 +173,6 @@ static void set_next_WP(struct Location *wp) loiter_sum = 0; loiter_total = 0; - // this is used to offset the shrinking longitude as we go towards the poles - float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); - // this is handy for the groundstation - wp_totalDistance = get_distance(¤t_loc, &next_WP); - wp_distance = wp_totalDistance; - target_bearing = get_bearing(¤t_loc, &next_WP); - nav_bearing = target_bearing; - - // to check if we have missed the WP - // ---------------------------- - old_target_bearing = target_bearing; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); - - gcs.print_current_waypoints(); -} - -static void set_guided_WP(void) -{ - SendDebug_P("MSG - set_guided_wp"); - - // copy the current location into the OldWP slot - // --------------------------------------- - prev_WP = current_loc; - - // Load the next_WP slot - // --------------------- - next_WP = guided_WP; - - // used to control FBW and limit the rate of climb - // ----------------------------------------------- - target_altitude = current_loc.alt; - offset_altitude = next_WP.alt - prev_WP.alt; - // this is used to offset the shrinking longitude as we go towards the poles float rads = (abs(next_WP.lat)/t7) * 0.0174532925; scaleLongDown = cos(rads); @@ -217,13 +190,15 @@ static void set_guided_WP(void) // set a new crosstrack bearing // ---------------------------- reset_crosstrack(); + + gcs.print_current_waypoints(); } // run this at setup on the ground // ------------------------------- void init_home() { - SendDebugln("MSG: init home"); + SendDebugln("MSG: init home"); // block until we get a good fix // ----------------------------- @@ -246,12 +221,6 @@ void init_home() // Save prev loc // ------------- next_WP = prev_WP = home; - - // Load home for a default guided_WP - // ------------- - guided_WP = home; - guided_WP.alt += g.RTL_altitude; - } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 7da8327970..1abbfe3693 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -3,7 +3,7 @@ /********************************************************************************/ // Command Event Handlers /********************************************************************************/ -static void +void handle_process_must() { // reset navigation integrators @@ -39,13 +39,13 @@ handle_process_must() case MAV_CMD_NAV_RETURN_TO_LAUNCH: do_RTL(); break; - + default: break; } } -static void +void handle_process_may() { switch(next_command.id){ @@ -82,7 +82,7 @@ handle_process_may() } } -static void handle_process_now() +void handle_process_now() { switch(next_command.id){ @@ -116,18 +116,27 @@ static void handle_process_now() } } -static void handle_no_commands() +void handle_no_commands() { - next_command = home; - next_command.alt = read_alt_to_hold(); - next_command.id = MAV_CMD_NAV_LOITER_UNLIM; + if (command_must_ID) + return; + + switch (control_mode){ + case LAND: + // don't get a new command + break; + + default: + set_mode(RTL); + break; + } } /********************************************************************************/ // Verify command Handlers /********************************************************************************/ -static bool verify_must() +bool verify_must() { switch(command_must_ID) { @@ -166,7 +175,7 @@ static bool verify_must() } } -static bool verify_may() +bool verify_may() { switch(command_may_ID) { case NO_COMMAND: @@ -186,18 +195,18 @@ static bool verify_may() default: gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); + return false; break; } - return false; } /********************************************************************************/ // Nav (Must) commands /********************************************************************************/ -static void do_RTL(void) +void do_RTL(void) { - control_mode = RTL; + control_mode = LOITER; crash_timer = 0; next_WP = home; @@ -213,7 +222,7 @@ static void do_RTL(void) Log_Write_Mode(control_mode); } -static void do_takeoff() +void do_takeoff() { set_next_WP(&next_command); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 @@ -227,28 +236,28 @@ static void do_takeoff() takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction } -static void do_nav_wp() +void do_nav_wp() { set_next_WP(&next_command); } -static void do_land() +void do_land() { set_next_WP(&next_command); } -static void do_loiter_unlimited() +void do_loiter_unlimited() { set_next_WP(&next_command); } -static void do_loiter_turns() +void do_loiter_turns() { set_next_WP(&next_command); loiter_total = next_command.p1 * 360; } -static void do_loiter_time() +void do_loiter_time() { set_next_WP(&next_command); loiter_time = millis(); @@ -258,7 +267,7 @@ static void do_loiter_time() /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ -static bool verify_takeoff() +bool verify_takeoff() { if (g_gps->ground_speed > 300){ if(hold_course == -1){ @@ -287,7 +296,7 @@ static bool verify_takeoff() } } -static bool verify_land() +bool verify_land() { // we don't verify landing - we never go to a new Must command after Land if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) @@ -314,7 +323,7 @@ static bool verify_land() return false; } -static bool verify_nav_wp() +bool verify_nav_wp() { hold_course = -1; update_crosstrack(); @@ -335,25 +344,25 @@ static bool verify_nav_wp() return false; } -static bool verify_loiter_unlim() +bool verify_loiter_unlim() { update_loiter(); calc_bearing_error(); return false; } -static bool verify_loiter_time() +bool verify_loiter_time() { update_loiter(); calc_bearing_error(); - if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds + if ((millis() - loiter_time) > (long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); return true; } return false; } -static bool verify_loiter_turns() +bool verify_loiter_turns() { update_loiter(); calc_bearing_error(); @@ -366,7 +375,7 @@ static bool verify_loiter_turns() return false; } -static bool verify_RTL() +bool verify_RTL() { if (wp_distance <= g.waypoint_radius) { gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home")); @@ -380,22 +389,22 @@ static bool verify_RTL() // Condition (May) commands /********************************************************************************/ -static void do_wait_delay() +void do_wait_delay() { condition_start = millis(); condition_value = next_command.lat * 1000; // convert to milliseconds } -static void do_change_alt() +void do_change_alt() { - condition_rate = next_command.lat; + condition_rate = next_command.p1; condition_value = next_command.alt; target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update next_WP.alt = condition_value; // For future nav calculations offset_altitude = 0; // For future nav calculations } -static void do_within_distance() +void do_within_distance() { condition_value = next_command.lat; } @@ -404,18 +413,23 @@ static void do_within_distance() // Verify Condition (May) commands /********************************************************************************/ -static bool verify_wait_delay() +bool verify_wait_delay() { - if ((unsigned)(millis() - condition_start) > condition_value){ + if ((millis() - condition_start) > condition_value){ condition_value = 0; return true; } return false; } -static bool verify_change_alt() +bool verify_change_alt() { - if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) { + //XXX this doesn't look right. How do you descend? + if(current_loc.alt >= condition_value) { + //Serial.printf_P(PSTR("alt, top:")); + //Serial.print(current_loc.alt, DEC); + //Serial.printf_P(PSTR("\t")); + //Serial.println(condition_value, DEC); condition_value = 0; return true; } @@ -423,7 +437,7 @@ static bool verify_change_alt() return false; } -static bool verify_within_distance() +bool verify_within_distance() { if (wp_distance < condition_value){ condition_value = 0; @@ -436,12 +450,12 @@ static bool verify_within_distance() // Do (Now) commands /********************************************************************************/ -static void do_loiter_at_location() +void do_loiter_at_location() { next_WP = current_loc; } -static void do_jump() +void do_jump() { struct Location temp; if(next_command.lat > 0) { @@ -453,22 +467,20 @@ static void do_jump() set_wp_with_index(temp, g.waypoint_index); g.waypoint_index.set_and_save(next_command.p1 - 1); - } else if (next_command.lat == -1) { - g.waypoint_index.set_and_save(next_command.p1 - 1); } } -static void do_change_speed() +void do_change_speed() { // Note: we have no implementation for commanded ground speed, only air speed and throttle if(next_command.alt > 0) g.airspeed_cruise.set_and_save(next_command.alt * 100); if(next_command.lat > 0) - g.throttle_cruise.set_and_save(next_command.lat); + g.throttle_cruise.set_and_save(next_command.lat * 100); } -static void do_set_home() +void do_set_home() { if(next_command.p1 == 1 && GPS_enabled) { init_home(); @@ -481,12 +493,12 @@ static void do_set_home() } } -static void do_set_servo() +void do_set_servo() { APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); } -static void do_set_relay() +void do_set_relay() { if (next_command.p1 == 1) { relay_on(); @@ -497,7 +509,7 @@ static void do_set_relay() } } -static void do_repeat_servo() +void do_repeat_servo() { event_id = next_command.p1 - 1; @@ -526,11 +538,11 @@ static void do_repeat_servo() } } -static void do_repeat_relay() +void do_repeat_relay() { event_id = RELAY_TOGGLE; event_timer = 0; event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_repeat = next_command.alt * 2; update_events(); -} +} diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index c0b2576e92..466ad2589a 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -2,15 +2,15 @@ // For changing active command mid-mission //---------------------------------------- -static void change_command(uint8_t cmd_index) +void change_command(uint8_t index) { - struct Location temp = get_wp_with_index(cmd_index); + struct Location temp = get_wp_with_index(index); if (temp.id > MAV_CMD_NAV_LAST ){ gcs.send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { command_must_index = NO_COMMAND; next_command.id = NO_COMMAND; - g.waypoint_index.set_and_save(cmd_index - 1); + g.waypoint_index.set_and_save(index - 1); load_next_command_from_EEPROM(); process_next_command(); } @@ -19,7 +19,7 @@ static void change_command(uint8_t cmd_index) // called by 10 Hz loop // -------------------- -static void update_commands(void) +void update_commands(void) { // This function loads commands into three buffers // when a new command is loaded, it is processed with process_XXX() @@ -34,7 +34,7 @@ static void update_commands(void) } // Other (eg GCS_Auto) modes may be implemented here } -static void verify_commands(void) +void verify_commands(void) { if(verify_must()){ command_must_index = NO_COMMAND; @@ -46,7 +46,7 @@ static void verify_commands(void) } } -static void load_next_command_from_EEPROM() +void load_next_command_from_EEPROM() { // fetch next command if the next command queue is empty // ----------------------------------------------------- @@ -64,7 +64,7 @@ static void load_next_command_from_EEPROM() } } -static void process_next_command() +void process_next_command() { // these are Navigation/Must commands // --------------------------------- @@ -115,7 +115,7 @@ static void process_next_command() /**************************************************/ // These functions implement the commands. /**************************************************/ -static void process_must() +void process_must() { gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -132,7 +132,7 @@ static void process_must() next_command.id = NO_COMMAND; } -static void process_may() +void process_may() { gcs.send_text_P(SEVERITY_LOW,PSTR("")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); @@ -145,7 +145,7 @@ static void process_may() next_command.id = NO_COMMAND; } -static void process_now() +void process_now() { handle_process_now(); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index bc1056e55f..003c1ea8e3 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -46,31 +46,13 @@ // AIRSPEED_SENSOR // AIRSPEED_RATIO // -#ifndef AIRSPEED_SENSOR -# define AIRSPEED_SENSOR DISABLED -#endif - +//#ifndef AIRSPEED_SENSOR +//# define AIRSPEED_SENSOR DISABLED +//#endif #ifndef AIRSPEED_RATIO # define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution #endif -////////////////////////////////////////////////////////////////////////////// -// Sonar -// - -#ifndef SONAR_ENABLED -# define SONAR_ENABLED DISABLED -#endif - -#ifndef SONAR_PIN -# define SONAR_PIN AN4 // AN5, AP_RANGEFINDER_PITOT_TUBE -#endif - -#ifndef SONAR_TYPE -# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL, -#endif - - ////////////////////////////////////////////////////////////////////////////// // HIL_PROTOCOL OPTIONAL // HIL_MODE OPTIONAL @@ -187,6 +169,9 @@ #ifndef MAG_ORIENTATION # define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #endif +#ifndef MAG_PROTOCOL +# define MAG_PROTOCOL MAG_PROTOCOL_5843 +#endif ////////////////////////////////////////////////////////////////////////////// @@ -199,7 +184,8 @@ ////////////////////////////////////////////////////////////////////////////// // Radio channel limits // -// Note that these are not called out in APM_Config.h.reference. +// Note that these are not called out in APM_Config.h.example as there +// is currently no configured behaviour for these channels. // #ifndef CH5_MIN # define CH5_MIN 1000 @@ -226,32 +212,7 @@ # define CH8_MAX 2000 #endif -////////////////////////////////////////////////////////////////////////////// -#ifndef RC_5_FUNCT -# define RC_5_FUNCT RC_5_FUNCT_NONE -#endif -#ifndef RC_6_FUNCT -# define RC_6_FUNCT RC_6_FUNCT_NONE -#endif -#ifndef RC_7_FUNCT -# define RC_7_FUNCT RC_7_FUNCT_NONE -#endif -#ifndef RC_8_FUNCT -# define RC_8_FUNCT RC_8_FUNCT_NONE -#endif -#ifndef FLAP_1_PERCENT -# define FLAP_1_PERCENT 0 -#endif -#ifndef FLAP_1_SPEED -# define FLAP_1_SPEED 255 -#endif -#ifndef FLAP_2_PERCENT -# define FLAP_2_PERCENT 0 -#endif -#ifndef FLAP_2_SPEED -# define FLAP_2_SPEED 255 -#endif ////////////////////////////////////////////////////////////////////////////// // FLIGHT_MODE // FLIGHT_MODE_CHANNEL @@ -295,14 +256,14 @@ #ifndef THROTTLE_FAILSAFE # define THROTTLE_FAILSAFE ENABLED #endif -#ifndef THROTTLE_FS_VALUE +#ifndef THROTTE_FS_VALUE # define THROTTLE_FS_VALUE 950 #endif #ifndef SHORT_FAILSAFE_ACTION -# define SHORT_FAILSAFE_ACTION 0 +# define SHORT_FAILSAFE_ACTION 2 #endif #ifndef LONG_FAILSAFE_ACTION -# define LONG_FAILSAFE_ACTION 0 +# define LONG_FAILSAFE_ACTION 2 #endif #ifndef GCS_HEARTBEAT_FAILSAFE # define GCS_HEARTBEAT_FAILSAFE DISABLED @@ -369,22 +330,6 @@ # define REVERSE_SWITCH ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// ENABLE ELEVON_MIXING -// -#ifndef ELEVON_MIXING -# define ELEVON_MIXING DISABLED -#endif -#ifndef ELEVON_REVERSE -# define ELEVON_REVERSE DISABLED -#endif -#ifndef ELEVON_CH1_REVERSE -# define ELEVON_CH1_REVERSE DISABLED -#endif -#ifndef ELEVON_CH2_REVERSE -# define ELEVON_CH2_REVERSE DISABLED -#endif - ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// @@ -611,12 +556,6 @@ ////////////////////////////////////////////////////////////////////////////// // Dataflash logging control // - -#ifndef LOGGING_ENABLED -# define LOGGING_ENABLED ENABLED -#endif - - #ifndef LOG_ATTITUDE_FAST # define LOG_ATTITUDE_FAST DISABLED #endif @@ -648,22 +587,6 @@ # define LOG_CUR DISABLED #endif -// calculate the default log_bitmask -#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) - -#define DEFAULT_LOG_BITMASK \ - LOGBIT(ATTITUDE_FAST) | \ - LOGBIT(ATTITUDE_MED) | \ - LOGBIT(GPS) | \ - LOGBIT(PM) | \ - LOGBIT(CTUN) | \ - LOGBIT(NTUN) | \ - LOGBIT(MODE) | \ - LOGBIT(RAW) | \ - LOGBIT(CMD) | \ - LOGBIT(CUR) - - #ifndef DEBUG_PORT # define DEBUG_PORT 0 #endif @@ -710,10 +633,6 @@ # define USE_CURRENT_ALT FALSE #endif -#ifndef INVERTED_FLIGHT_PWM -# define INVERTED_FLIGHT_PWM 1750 -#endif - ////////////////////////////////////////////////////////////////////////////// // Developer Items // @@ -724,17 +643,3 @@ #endif #define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE) -// use this to enable servos in HIL mode -#ifndef HIL_SERVOS -# define HIL_SERVOS DISABLED -#endif - -// use this to completely disable the CLI -#ifndef CLI_ENABLED -# define CLI_ENABLED ENABLED -#endif - -// delay to prevent Xbee bricking, in milliseconds -#ifndef MAVLINK_TELEMETRY_PORT_DELAY -# define MAVLINK_TELEMETRY_PORT_DELAY 2000 -#endif diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index 4c679f2e52..b760195eb9 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -1,11 +1,9 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -static void read_control_switch() +void read_control_switch() { byte switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition){ - set_mode(flight_modes[switchPosition]); + set_mode(g.flight_modes[switchPosition]); oldSwitchPosition = switchPosition; prev_WP = current_loc; @@ -14,15 +12,9 @@ static void read_control_switch() // ------------------------- reset_I(); } - - if (g.inverted_flight_ch != 0) { - // if the user has configured an inverted flight channel, then - // fly upside down when that channel goes above INVERTED_FLIGHT_PWM - inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); - } } -static byte readSwitch(void){ +byte readSwitch(void){ uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; @@ -32,7 +24,7 @@ static byte readSwitch(void){ return 0; } -static void reset_control_switch() +void reset_control_switch() { oldSwitchPosition = 0; read_control_switch(); @@ -40,23 +32,52 @@ static void reset_control_switch() SendDebugln(oldSwitchPosition , DEC); } -static void update_servo_switches() +void update_servo_switches() { - if (!g.switch_enable) { - // switches are disabled in EEPROM (see SWITCH_ENABLE option) - // this means the EEPROM control of all channel reversal is enabled - return; - } // up is reverse // up is elevon - g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode - if (g.mix_mode == 0) { - g.channel_roll.set_reverse((PINE & 128) ? true : false); - g.channel_pitch.set_reverse((PINE & 64) ? true : false); - g.channel_rudder.set_reverse((PINL & 64) ? true : false); + mix_mode = (PINL & 128) ? 1 : 0; + if (mix_mode == 0) { + reverse_roll = (PINE & 128) ? true : false; + reverse_pitch = (PINE & 64) ? true : false; + reverse_rudder = (PINL & 64) ? true : false; } else { - g.reverse_elevons = (PINE & 128) ? true : false; - g.reverse_ch1_elevon = (PINE & 64) ? true : false; - g.reverse_ch2_elevon = (PINL & 64) ? true : false; + reverse_elevons = (PINE & 128) ? -1 : 1; + reverse_ch1_elevon = (PINE & 64) ? -1 : 1; + reverse_ch2_elevon = (PINL & 64) ? -1 : 1; + } +} + + +bool trim_flag; +unsigned long trim_timer; + +// read at 10 hz +// set this to your trainer switch +void read_trim_switch() +{ + // switch is engaged + if (g.rc_7.control_in > 800){ + if(trim_flag == false){ + // called once + trim_timer = millis(); + } + trim_flag = true; + + }else{ // switch is disengaged + + if(trim_flag){ + // switch was just released + if((millis() - trim_timer) > 2000){ + + g.throttle_cruise.set_and_save(g.channel_throttle.control_in); + g.angle_of_attack.set_and_save(dcm.pitch_sensor); + g.airspeed_cruise.set_and_save(airspeed); + + } else { + + } + trim_flag = false; + } } } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b48deed2da..43ac0226d6 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -10,9 +10,11 @@ #define DEBUG 0 #define LOITER_RANGE 60 // for calculating power outside of loiter radius -#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. +#define ROLL_SERVO_MAX 4500 +#define PITCH_SERVO_MAX 4500 +#define RUDDER_SERVO_MAX 4500 -// failsafe +// failsafe // ---------------------- #define FAILSAFE_NONE 0 #define FAILSAFE_SHORT 1 @@ -30,6 +32,10 @@ #define T6 1000000 #define T7 10000000 +//MAGNETOMETER +#define MAG_PROTOCOL_5843 0 +#define MAG_PROTOCOL_5883L 1 + // GPS type codes - use the names, not the numbers #define GPS_PROTOCOL_NONE -1 #define GPS_PROTOCOL_NMEA 0 @@ -60,20 +66,6 @@ #define CH_RUDDER CH_4 #define CH_YAW CH_4 -#define RC_5_FUNCT_NONE 0 -#define RC_5_FUNCT_AILERON 1 -#define RC_5_FUNCT_FLAP_AUTO 2 -#define RC_5_FUNCT_FLAPERON 3 - -#define RC_6_FUNCT_NONE 0 -#define RC_6_FUNCT_AILERON 1 -#define RC_6_FUNCT_FLAP_AUTO 2 -#define RC_6_FUNCT_FLAPERON 3 - -#define RC_7_FUNCT_NONE 0 - -#define RC_8_FUNCT_NONE 0 - // HIL enumerations #define HIL_PROTOCOL_XPLANE 1 #define HIL_PROTOCOL_MAVLINK 2 @@ -98,15 +90,12 @@ #define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle #define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed -#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed - // Fly By Wire B and Fly By Wire C require airspeed sensor + // Fly By Wire B = Fly By Wire A if you have AIRSPEED_SENSOR 0 #define AUTO 10 #define RTL 11 #define LOITER 12 -//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM -//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM -#define GUIDED 15 -#define INITIALISING 16 // in startup routines +#define TAKEOFF 13 +#define LAND 14 // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library @@ -129,10 +118,6 @@ #define MAV_CMD_CONDITION_YAW 23 // GCS Message ID's -/// NOTE: to ensure we never block on sending MAVLink messages -/// please keep each MSG_ to a single MAVLink message. If need be -/// create new MSG_ IDs for additional messages on the same -/// stream #define MSG_ACKNOWLEDGE 0x00 #define MSG_HEARTBEAT 0x01 #define MSG_ATTITUDE 0x02 @@ -143,10 +128,9 @@ #define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed #define MSG_VERSION_REQUEST 0x08 #define MSG_VERSION 0x09 -#define MSG_EXTENDED_STATUS1 0x0a -#define MSG_EXTENDED_STATUS2 0x0b -#define MSG_CPU_LOAD 0x0c -#define MSG_NAV_CONTROLLER_OUTPUT 0x0d +#define MSG_EXTENDED_STATUS 0x0a +#define MSG_CPU_LOAD 0x0b +#define MSG_NAV_CONTROLLER_OUTPUT 0x0c #define MSG_COMMAND_REQUEST 0x20 #define MSG_COMMAND_UPLOAD 0x21 @@ -169,11 +153,9 @@ #define MSG_RADIO_OUT 0x53 #define MSG_RADIO_IN 0x54 -#define MSG_RAW_IMU1 0x60 -#define MSG_RAW_IMU2 0x61 -#define MSG_RAW_IMU3 0x62 -#define MSG_GPS_STATUS 0x63 -#define MSG_GPS_RAW 0x64 +#define MSG_RAW_IMU 0x60 +#define MSG_GPS_STATUS 0x61 +#define MSG_GPS_RAW 0x62 #define MSG_SERVO_OUT 0x70 @@ -191,7 +173,6 @@ #define MSG_POSITION_SET 0xb2 #define MSG_ATTITUDE_SET 0xb3 #define MSG_LOCAL_LOCATION 0xb4 -#define MSG_RETRY_DEFERRED 0xff #define SEVERITY_LOW 1 #define SEVERITY_MEDIUM 2 @@ -224,6 +205,7 @@ #define MASK_LOG_RAW (1<<7) #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CUR (1<<9) +#define MASK_LOG_SET_DEFAULTS (1<<15) // Waypoint Modes // ---------------- @@ -264,11 +246,7 @@ // sonar -#define MAX_SONAR_XL 0 -#define MAX_SONAR_LV 1 #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters -#define AN4 4 -#define AN5 5 // Hardware Parameters #define SLIDE_SWITCH_PIN 40 @@ -289,6 +267,3 @@ #define ONBOARD_PARAM_NAME_LENGTH 15 #define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe - -// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1) -#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1) diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index acd60e9cfc..e0445f06d1 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void failsafe_short_on_event() +void failsafe_short_on_event() { // This is how to handle a short loss of control signal failsafe. failsafe = FAILSAFE_SHORT; @@ -18,7 +18,7 @@ static void failsafe_short_on_event() case AUTO: case LOITER: - if(g.short_fs_action == 1) { + if(g.short_fs_action != 1) { set_mode(RTL); } break; @@ -32,11 +32,10 @@ static void failsafe_short_on_event() SendDebugln(control_mode, DEC); } -static void failsafe_long_on_event() +void failsafe_long_on_event() { - // This is how to handle a long loss of control signal failsafe. + // This is how to handle a short loss of control signal failsafe. SendDebug_P("Failsafe - Long event on"); - APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC switch(control_mode) { case MANUAL: @@ -49,7 +48,7 @@ static void failsafe_long_on_event() case AUTO: case LOITER: case CIRCLE: - if(g.long_fs_action == 1) { + if(g.long_fs_action != 1) { set_mode(RTL); } break; @@ -60,7 +59,7 @@ static void failsafe_long_on_event() } } -static void failsafe_short_off_event() +void failsafe_short_off_event() { // We're back in radio contact SendDebug_P("Failsafe - Short event off"); @@ -75,16 +74,15 @@ static void failsafe_short_off_event() reset_I(); } -#if BATTERY_EVENT == ENABLED -static void low_battery_event(void) +void low_battery_event(void) { gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); set_mode(RTL); g.throttle_cruise = THROTTLE_CRUISE; } -#endif -static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY + +void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY { if(event_repeat == 0 || (millis() - event_timer) < event_delay) return; @@ -110,17 +108,17 @@ static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_ } } -static void relay_on() +void relay_on() { PORTL |= B00000100; } -static void relay_off() +void relay_off() { PORTL &= ~B00000100; } -static void relay_toggle() +void relay_toggle() { PORTL ^= B00000100; } diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 487ffe4a3b..6090510d23 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -3,7 +3,7 @@ //**************************************************************** // Function that will calculate the desired direction to fly and distance //**************************************************************** -static void navigate() +void navigate() { // do not navigate with corrupt data // --------------------------------- @@ -63,7 +63,7 @@ void calc_distance_error() } #endif -static void calc_airspeed_errors() +void calc_airspeed_errors() { // XXX excess casting here if(control_mode>=AUTO && airspeed_nudge > 0) { @@ -75,7 +75,7 @@ static void calc_airspeed_errors() } } -static void calc_bearing_error() +void calc_bearing_error() { if(takeoff_complete == true || g.compass_enabled == true) { bearing_error = nav_bearing - dcm.yaw_sensor; @@ -89,7 +89,7 @@ static void calc_bearing_error() bearing_error = wrap_180(bearing_error); } -static void calc_altitude_error() +void calc_altitude_error() { if(control_mode == AUTO && offset_altitude != 0) { // limit climb rates @@ -127,39 +127,38 @@ static void calc_altitude_error() altitude_error = target_altitude - current_loc.alt; } -static long wrap_360(long error) +long wrap_360(long error) { if (error > 36000) error -= 36000; if (error < 0) error += 36000; return error; } -static long wrap_180(long error) +long wrap_180(long error) { if (error > 18000) error -= 36000; if (error < -18000) error += 36000; return error; } -static void update_loiter() +void update_loiter() { float power; if(wp_distance <= g.loiter_radius){ power = float(wp_distance) / float(g.loiter_radius); - power = constrain(power, 0.5, 1); nav_bearing += (int)(9000.0 * (2.0 + power)); + }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); - power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); + power = constrain(power, 0, 1); nav_bearing -= power * 9000; }else{ update_crosstrack(); loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit - } -/* + if (wp_distance < g.loiter_radius){ nav_bearing += 9000; }else{ @@ -167,27 +166,35 @@ static void update_loiter() } update_crosstrack(); -*/ nav_bearing = wrap_360(nav_bearing); } -static void update_crosstrack(void) +void update_crosstrack(void) { // Crosstrack Error // ---------------- if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line + crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing); } } -static void reset_crosstrack() +void reset_crosstrack() { crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } -static long get_distance(struct Location *loc1, struct Location *loc2) +long get_altitude_above_home(void) // TO DO - this function has been deprecated - check if any useage vs removal +{ + // This is the altitude above the home location + // The GPS gives us altitude at Sea Level + // if you slope soar, you should see a negative number sometimes + // ------------------------------------------------------------- + return current_loc.alt - home.alt; +} + +long get_distance(struct Location *loc1, struct Location *loc2) { if(loc1->lat == 0 || loc1->lng == 0) return -1; @@ -198,7 +205,12 @@ static long get_distance(struct Location *loc1, struct Location *loc2) return sqrt(sq(dlat) + sq(dlong)) * .01113195; } -static long get_bearing(struct Location *loc1, struct Location *loc2) +long get_alt_distance(struct Location *loc1, struct Location *loc2) +{ + return abs(loc1->alt - loc2->alt); +} + +long get_bearing(struct Location *loc1, struct Location *loc2) { long off_x = loc2->lng - loc1->lng; long off_y = (loc2->lat - loc1->lat) * scaleLongUp; @@ -206,3 +218,14 @@ static long get_bearing(struct Location *loc1, struct Location *loc2) if (bearing < 0) bearing += 36000; return bearing; } + +Vector3f get_location_vector(struct Location *uav, struct Location *target) +{ + Vector3 v; + v.x = (target->lng-uav->lng) * cos((uav->lat+target->lat)/2)*.01113195; + v.y = (target->lat-uav->lat)*.01113195; + v.z = (uav->alt-target->alt); + return v; +} + + diff --git a/ArduPlane/planner.pde b/ArduPlane/planner.pde index 3154f18084..9497110381 100644 --- a/ArduPlane/planner.pde +++ b/ArduPlane/planner.pde @@ -8,19 +8,18 @@ static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details -static const struct Menu::command planner_menu_commands[] PROGMEM = { +const struct Menu::command planner_menu_commands[] PROGMEM = { {"gcs", planner_gcs}, }; // A Macro to create the Menu MENU(planner_menu, "planner", planner_menu_commands); -static int8_t +int8_t planner_mode(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); planner_menu.run(); - return 0; } static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv) @@ -45,6 +44,4 @@ planner_gcs(uint8_t argc, const Menu::arg *argv) loopcount++; } } - return 0; -} - +} diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 748b7a9228..0e0028ab90 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -2,18 +2,21 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling -static void init_rc_in() +void init_rc_in() { // set rc reversing update_servo_switches(); + g.channel_roll.set_reverse(reverse_roll); + g.channel_pitch.set_reverse(reverse_pitch); + g.channel_rudder.set_reverse(reverse_rudder); // set rc channel ranges - g.channel_roll.set_angle(SERVO_MAX); - g.channel_pitch.set_angle(SERVO_MAX); - g.channel_rudder.set_angle(SERVO_MAX); + g.channel_roll.set_angle(ROLL_SERVO_MAX); + g.channel_pitch.set_angle(PITCH_SERVO_MAX); + g.channel_rudder.set_angle(RUDDER_SERVO_MAX); g.channel_throttle.set_range(0, 100); // set rc dead zones @@ -23,27 +26,13 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - if (g.rc_5_funct == RC_5_FUNCT_AILERON) { - g.rc_5.set_angle(SERVO_MAX); - } else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) { - g.rc_5.set_range(0,100); - } else { - g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - if (g.rc_6_funct == RC_6_FUNCT_AILERON) { - g.rc_6.set_angle(SERVO_MAX); - } else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) { - g.rc_6.set_range(0,100); - } else { - g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here - } - - g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here + g.rc_5.set_range(0,1000); + g.rc_6.set_range(0,1000); + g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); } -static void init_rc_out() +void init_rc_out() { APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); @@ -68,17 +57,17 @@ static void init_rc_out() APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); } -static void read_radio() +void read_radio() { ch1_temp = APM_RC.InputCh(CH_ROLL); ch2_temp = APM_RC.InputCh(CH_PITCH); - if(g.mix_mode == 0){ + if(mix_mode == 0){ g.channel_roll.set_pwm(ch1_temp); g.channel_pitch.set_pwm(ch2_temp); }else{ - g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); - g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); + g.channel_roll.set_pwm(reverse_elevons * (reverse_ch2_elevon * int(ch2_temp - elevon2_trim) - reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500); + g.channel_pitch.set_pwm((reverse_ch2_elevon * int(ch2_temp - elevon2_trim) + reverse_ch1_elevon * int(ch1_temp - elevon1_trim)) / 2 + 1500); } g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); @@ -98,7 +87,7 @@ static void read_radio() g.channel_throttle.servo_out = g.channel_throttle.control_in; if (g.channel_throttle.servo_out > 50) { - if(g.airspeed_enabled == true) { + if(airspeed_enabled == true) { airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); } else { throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); @@ -117,7 +106,7 @@ static void read_radio() */ } -static void control_failsafe(uint16_t pwm) +void control_failsafe(uint16_t pwm) { if(g.throttle_fs_enabled == 0) return; @@ -132,7 +121,7 @@ static void control_failsafe(uint16_t pwm) //Check for failsafe and debounce funky reads } else if (g.throttle_fs_enabled) { - if (pwm < (unsigned)g.throttle_fs_value){ + if (pwm < g.throttle_fs_value){ // we detect a failsafe from radio // throttle has dropped below the mark failsafeCounter++; @@ -164,18 +153,15 @@ static void control_failsafe(uint16_t pwm) } } -static void trim_control_surfaces() +void trim_control_surfaces() { read_radio(); // Store control surface trim values // --------------------------------- - if(g.mix_mode == 0){ + if(mix_mode == 0){ g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel - }else{ elevon1_trim = ch1_temp; elevon2_trim = ch2_temp; @@ -191,11 +177,9 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); } -static void trim_radio() +void trim_radio() { for (int y = 0; y < 30; y++) { read_radio(); @@ -203,13 +187,11 @@ static void trim_radio() // Store the trim values // --------------------- - if(g.mix_mode == 0){ + if(mix_mode == 0){ g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -225,7 +207,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom(); - if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom(); } diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 3f7001d1ad..d74ffcb060 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -5,17 +5,21 @@ void ReadSCP1000(void) {} -static void init_barometer(void) +void init_barometer(void) { - int flashcount = 0; + int flashcount; long ground_pressure = 0; int ground_temperature; + #if HIL_MODE == HIL_MODE_SENSORS + hil.update(); // look for inbound hil packets for initialization + #endif + while(ground_pressure == 0){ barometer.Read(); // Get initial data from absolute pressure sensor ground_pressure = barometer.Press; ground_temperature = barometer.Temp; - mavlink_delay(20); + delay(20); //Serial.printf("barometer.Press %ld\n", barometer.Press); } @@ -29,7 +33,7 @@ static void init_barometer(void) ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l; ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; - mavlink_delay(20); + delay(20); if(flashcount == 5) { digitalWrite(C_LED_PIN, LOW); digitalWrite(A_LED_PIN, HIGH); @@ -49,11 +53,11 @@ static void init_barometer(void) g.ground_temperature.set_and_save(ground_temperature / 10.0f); abs_pressure = ground_pressure; - Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); +Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); + SendDebugln_P("barometer calibration complete."); } -static long read_barometer(void) +long read_barometer(void) { float x, scaling, temp; @@ -69,38 +73,30 @@ static long read_barometer(void) } // in M/S * 100 -static void read_airspeed(void) +void read_airspeed(void) { #if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed - if (g.airspeed_offset == 0) { - // runtime enabling of airspeed, we need to do instant - // calibration before we can use it. This isn't as - // accurate as the 50 point average in zero_airspeed(), - // but it is better than using it uncalibrated - airspeed_raw = (float)adc.Ch(AIRSPEED_CH); - g.airspeed_offset.set_and_save(airspeed_raw); - } airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75); - airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0); + airspeed_pressure = max(((int)airspeed_raw - airspeed_offset), 0); airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100; #endif calc_airspeed_errors(); } -static void zero_airspeed(void) +void zero_airspeed(void) { airspeed_raw = (float)adc.Ch(AIRSPEED_CH); - for(int c = 0; c < 10; c++){ + for(int c = 0; c < 50; c++){ delay(20); airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10); } - g.airspeed_offset.set_and_save(airspeed_raw); + airspeed_offset = airspeed_raw; } #endif // HIL_MODE != HIL_MODE_ATTITUDE -static void read_battery(void) +void read_battery(void) { battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; @@ -118,7 +114,7 @@ static void read_battery(void) current_total += current_amps * (float)delta_ms_medium_loop * 0.000278; } - #if BATTERY_EVENT == ENABLED + #if BATTERY_EVENT == 1 if(battery_voltage < LOW_VOLTAGE) low_battery_event(); if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); #endif diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 2841e8b787..7d35d953c6 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -1,7 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if CLI_ENABLED == ENABLED - // Functions called from the setup menu static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); @@ -13,7 +11,7 @@ static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); // Command/function table for the setup menu -static const struct Menu::command setup_menu_commands[] PROGMEM = { +const struct Menu::command setup_menu_commands[] PROGMEM = { // command function called // ======= =============== {"reset", setup_factory}, @@ -30,7 +28,7 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = { MENU(setup_menu, "setup", setup_menu_commands); // Called from the top-level menu to run the setup menu. -static int8_t +int8_t setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance @@ -43,7 +41,6 @@ setup_mode(uint8_t argc, const Menu::arg *argv) // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); - return 0; } // Print the current configuration. @@ -51,6 +48,7 @@ setup_mode(uint8_t argc, const Menu::arg *argv) static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { + uint8_t i; // clear the area print_blanks(8); @@ -75,6 +73,8 @@ setup_show(uint8_t argc, const Menu::arg *argv) static int8_t setup_factory(uint8_t argc, const Menu::arg *argv) { + + uint8_t i; int c; Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: ")); @@ -185,7 +185,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { - byte switchPosition, mode = 0; + byte switchPosition, oldSwitchPosition, mode; Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); @@ -201,10 +201,10 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) if (oldSwitchPosition != switchPosition){ // force position 5 to MANUAL if (switchPosition > 4) { - flight_modes[switchPosition] = MANUAL; + g.flight_modes[switchPosition] = MANUAL; } // update our current mode - mode = flight_modes[switchPosition]; + mode = g.flight_modes[switchPosition]; // update the user print_switch(switchPosition, mode); @@ -228,11 +228,13 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode != FLY_BY_WIRE_B && mode != AUTO && mode != RTL && - mode != LOITER) + mode != LOITER && + mode != TAKEOFF && + mode != LAND) { if (mode < MANUAL) - mode = LOITER; - else if (mode >LOITER) + mode = LAND; + else if (mode >LAND) mode = MANUAL; else mode += radioInputSwitch; @@ -243,7 +245,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) mode = MANUAL; // save new mode - flight_modes[switchPosition] = mode; + g.flight_modes[switchPosition] = mode; // print new mode print_switch(switchPosition, mode); @@ -252,8 +254,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) // escape hatch if(Serial.available() > 0){ // save changes - for (mode=0; mode<6; mode++) - flight_modes[mode].save(); + g.flight_modes.save(); report_flight_modes(); print_done(); return (0); @@ -266,13 +267,13 @@ setup_declination(uint8_t argc, const Menu::arg *argv) { compass.set_declination(radians(argv[1].f)); report_compass(); - return 0; } static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) { + uint8_t i; int c; Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: ")); @@ -291,13 +292,9 @@ static int8_t setup_compass(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("on"))) { - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - g.compass_enabled = false; - } else { - g.compass_enabled = true; - } + g.compass_enabled = true; + bool junkbool = compass.init(); + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.compass_enabled = false; @@ -326,11 +323,50 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv) return 0; } +/***************************************************************************/ +// CLI defaults +/***************************************************************************/ + +void +default_flight_modes() +{ + g.flight_modes[0] = FLIGHT_MODE_1; + g.flight_modes[1] = FLIGHT_MODE_2; + g.flight_modes[2] = FLIGHT_MODE_3; + g.flight_modes[3] = FLIGHT_MODE_4; + g.flight_modes[4] = FLIGHT_MODE_5; + g.flight_modes[5] = FLIGHT_MODE_6; + g.flight_modes.save(); +} + +void +default_log_bitmask() +{ + + // convenience macro for testing LOG_* and setting LOGBIT_* + #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) + + g.log_bitmask = + LOGBIT(ATTITUDE_FAST) | + LOGBIT(ATTITUDE_MED) | + LOGBIT(GPS) | + LOGBIT(PM) | + LOGBIT(CTUN) | + LOGBIT(NTUN) | + LOGBIT(MODE) | + LOGBIT(RAW) | + LOGBIT(CMD) | + LOGBIT(CUR); + #undef LOGBIT + + g.log_bitmask.save(); +} + /***************************************************************************/ // CLI reports /***************************************************************************/ -static void report_batt_monitor() +void report_batt_monitor() { //print_blanks(2); Serial.printf_P(PSTR("Batt Mointor\n")); @@ -342,7 +378,7 @@ static void report_batt_monitor() if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); print_blanks(2); } -static void report_radio() +void report_radio() { //print_blanks(2); Serial.printf_P(PSTR("Radio\n")); @@ -352,7 +388,7 @@ static void report_radio() print_blanks(2); } -static void report_gains() +void report_gains() { //print_blanks(2); Serial.printf_P(PSTR("Gains\n")); @@ -370,7 +406,7 @@ static void report_gains() Serial.printf_P(PSTR("nav roll:\n")); print_PID(&g.pidNavRoll); - Serial.printf_P(PSTR("nav pitch airspeed:\n")); + Serial.printf_P(PSTR("nav pitch airpseed:\n")); print_PID(&g.pidNavPitchAirspeed); Serial.printf_P(PSTR("energry throttle:\n")); @@ -382,7 +418,7 @@ static void report_gains() print_blanks(2); } -static void report_xtrack() +void report_xtrack() { //print_blanks(2); Serial.printf_P(PSTR("Crosstrack\n")); @@ -395,7 +431,7 @@ static void report_xtrack() print_blanks(2); } -static void report_throttle() +void report_throttle() { //print_blanks(2); Serial.printf_P(PSTR("Throttle\n")); @@ -414,7 +450,7 @@ static void report_throttle() print_blanks(2); } -static void report_imu() +void report_imu() { //print_blanks(2); Serial.printf_P(PSTR("IMU\n")); @@ -425,32 +461,16 @@ static void report_imu() print_blanks(2); } -static void report_compass() +void report_compass() { //print_blanks(2); - Serial.printf_P(PSTR("Compass: ")); - - switch (compass.product_id) { - case AP_COMPASS_TYPE_HMC5883L: - Serial.println_P(PSTR("HMC5883L")); - break; - case AP_COMPASS_TYPE_HMC5843: - Serial.println_P(PSTR("HMC5843")); - break; - case AP_COMPASS_TYPE_HIL: - Serial.println_P(PSTR("HIL")); - break; - default: - Serial.println_P(PSTR("??")); - break; - } - + Serial.printf_P(PSTR("Compass\n")); print_divider(); print_enabled(g.compass_enabled); // mag declination - Serial.printf_P(PSTR("Mag Declination: %4.4f\n"), + Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), degrees(compass.get_declination())); Vector3f offsets = compass.get_offsets(); @@ -463,14 +483,14 @@ static void report_compass() print_blanks(2); } -static void report_flight_modes() +void report_flight_modes() { //print_blanks(2); Serial.printf_P(PSTR("Flight modes\n")); print_divider(); for(int i = 0; i < 6; i++ ){ - print_switch(i, flight_modes[i]); + print_switch(i, g.flight_modes[i]); } print_blanks(2); } @@ -479,7 +499,7 @@ static void report_flight_modes() // CLI utilities /***************************************************************************/ -static void +void print_PID(PID * pid) { Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), @@ -489,7 +509,7 @@ print_PID(PID * pid) (long)pid->imax()); } -static void +void print_radio_values() { Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max); @@ -503,20 +523,20 @@ print_radio_values() } -static void +void print_switch(byte p, byte m) { Serial.printf_P(PSTR("Pos %d: "),p); Serial.println(flight_mode_strings[m]); } -static void +void print_done() { Serial.printf_P(PSTR("\nSaved Settings\n\n")); } -static void +void print_blanks(int num) { while(num > 0){ @@ -525,7 +545,7 @@ print_blanks(int num) } } -static void +void print_divider(void) { for (int i = 0; i < 40; i++) { @@ -534,7 +554,7 @@ print_divider(void) Serial.println(""); } -static int8_t +int8_t radio_input_switch(void) { static int8_t bouncer = 0; @@ -561,9 +581,9 @@ radio_input_switch(void) } -static void zero_eeprom(void) +void zero_eeprom(void) { - byte b = 0; + byte b; Serial.printf_P(PSTR("\nErasing EEPROM\n")); for (int i = 0; i < EEPROM_MAX_ADDR; i++) { eeprom_write_byte((uint8_t *) i, b); @@ -571,7 +591,7 @@ static void zero_eeprom(void) Serial.printf_P(PSTR("done\n")); } -static void print_enabled(bool b) +void print_enabled(bool b) { if(b) Serial.printf_P(PSTR("en")); @@ -580,7 +600,7 @@ static void print_enabled(bool b) Serial.printf_P(PSTR("abled\n")); } -static void +void print_accel_offsets(void) { Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), @@ -589,7 +609,7 @@ print_accel_offsets(void) (float)imu.az()); } -static void +void print_gyro_offsets(void) { Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), @@ -599,4 +619,3 @@ print_gyro_offsets(void) } -#endif // CLI_ENABLED diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index cbe7ddadd2..b447ef3b57 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -6,13 +6,11 @@ The init_ardupilot function processes everything we need for an in - air restart *****************************************************************************/ -#if CLI_ENABLED == ENABLED - // Functions called from the top-level menu -static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp -static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde +extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde +extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp +extern int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -30,7 +28,7 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) } // Command/function table for the top-level menu. -static const struct Menu::command main_menu_commands[] PROGMEM = { +const struct Menu::command main_menu_commands[] PROGMEM = { // command function called // ======= =============== {"logs", process_logs}, @@ -41,11 +39,9 @@ static const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "APM trunk", main_menu_commands); +MENU(main_menu, "ArduPilotMega", main_menu_commands); -#endif // CLI_ENABLED - -static void init_ardupilot() +void init_ardupilot() { // Console serial port // @@ -77,6 +73,16 @@ static void init_ardupilot() Serial1.begin(38400, 128, 16); #endif + // Telemetry port. + // + // Not used if telemetry is going to the console. + // + // XXX for unidirectional protocols, we could (should) minimize + // the receive buffer, and the transmit buffer could also be + // shrunk for protocols that don't send large messages. + // + Serial3.begin(SERIAL3_BAUD, 128, 128); + Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %lu\n"), freeRAM()); @@ -86,10 +92,7 @@ static void init_ardupilot() // if (!g.format_version.load()) { - Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n")); - delay(100); // wait for serial msg to flush - - AP_Var::erase_all(); + Serial.println("\n\nEEPROM blank - all parameters are reset to default values.\n"); // save the current format version g.format_version.set_and_save(Parameters::k_format_version); @@ -99,7 +102,6 @@ static void init_ardupilot() Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" "\n\nForcing complete parameter reset..."), g.format_version.get(), Parameters::k_format_version); - delay(100); // wait for serial msg to flush // erase all parameters AP_Var::erase_all(); @@ -114,48 +116,45 @@ static void init_ardupilot() AP_Var::load_all(); Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); - Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"), - AP_Var::get_memory_use(), (unsigned)g.num_resets); + Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); } - // keep a record of how many resets have happened. This can be - // used to detect in-flight resets - g.num_resets.set_and_save(g.num_resets+1); - - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + if (!g.flight_modes.load()) { + default_flight_modes(); + } + if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) { + default_log_bitmask(); + } mavlink_system.sysid = g.sysid_this_mav; +#if CAMERA == ENABLED + init_camera(); +#endif #if HIL_MODE != HIL_MODE_ATTITUDE adc.Init(); // APM ADC library initialization barometer.Init(); // APM Abs Pressure sensor initialization + // Autodetect airspeed sensor + if ((adc.Ch(AIRSPEED_CH) > 2000)&&(adc.Ch(AIRSPEED_CH) < 2900)) + { + airspeed_enabled = false; + } + else + { + airspeed_enabled = true; + } + if (g.compass_enabled==true) { - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - g.compass_enabled = false; - } else { - dcm.set_compass(&compass); - compass.get_offsets(); // load offsets to account for airframe magnetic interference - } + dcm.set_compass(&compass); + bool junkbool = compass.init(); + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference + //compass.set_declination(ToRad(get(PARAM_DECLINATION))); // TODO fix this to have a UI // set local difference between magnetic north and true north } - /* - Init is depricated - Jason - if(g.sonar_enabled){ - sonar.init(SONAR_PIN, &adc); - Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC); - } - */ +#else + airspeed_enabled = true; #endif DataFlash.Init(); // DataFlash log initialization @@ -163,7 +162,6 @@ static void init_ardupilot() // Do GPS init g_gps = &g_gps_driver; g_gps->init(); // GPS Initialization - g_gps->callback = mavlink_delay; // init the GCS #if GCS_PORT == 3 @@ -210,7 +208,6 @@ static void init_ardupilot() // the system in an odd state, we don't let the user exit the top // menu; they must reset in order to fly. // -#if CLI_ENABLED == ENABLED if (digitalRead(SLIDE_SWITCH_PIN) == 0) { digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED Serial.printf_P(PSTR("\n" @@ -224,7 +221,6 @@ static void init_ardupilot() main_menu.run(); } } -#endif // CLI_ENABLED if(g.log_bitmask != 0){ @@ -272,8 +268,6 @@ static void init_ardupilot() Log_Write_Startup(TYPE_GROUNDSTART_MSG); } - set_mode(MANUAL); - // set the correct flight mode // --------------------------- reset_control_switch(); @@ -282,10 +276,8 @@ static void init_ardupilot() //******************************************************************************** //This function does all the calibrations, etc. that we need during a ground start //******************************************************************************** -static void startup_ground(void) +void startup_ground(void) { - set_mode(INITIALISING); - gcs.send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); #if(GROUND_START_DELAY > 0) @@ -314,7 +306,7 @@ static void startup_ground(void) trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. #if HIL_MODE != HIL_MODE_ATTITUDE -if (g.airspeed_enabled == true) +if (airspeed_enabled == true) { // initialize airspeed sensor // -------------------------- @@ -357,7 +349,7 @@ else gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } -static void set_mode(byte mode) +void set_mode(byte mode) { if(control_mode == mode){ // don't switch modes if we are already in the correct mode. @@ -371,11 +363,18 @@ static void set_mode(byte mode) switch(control_mode) { - case INITIALISING: case MANUAL: + break; + case CIRCLE: + break; + case STABILIZE: + break; + case FLY_BY_WIRE_A: + break; + case FLY_BY_WIRE_B: break; @@ -391,8 +390,10 @@ static void set_mode(byte mode) do_loiter_at_location(); break; - case GUIDED: - set_guided_WP(); + case TAKEOFF: + break; + + case LAND: break; default: @@ -407,7 +408,7 @@ static void set_mode(byte mode) Log_Write_Mode(control_mode); } -static void check_long_failsafe() +void check_long_failsafe() { // only act on changes // ------------------- @@ -432,7 +433,7 @@ static void check_long_failsafe() } } -static void check_short_failsafe() +void check_short_failsafe() { // only act on changes // ------------------- @@ -441,7 +442,7 @@ static void check_short_failsafe() failsafe_short_on_event(); } } - + if(failsafe == FAILSAFE_SHORT){ if(!ch3_failsafe) { failsafe_short_off_event(); @@ -450,19 +451,20 @@ static void check_short_failsafe() } -static void startup_IMU_ground(void) +void startup_IMU_ground(void) { #if HIL_MODE != HIL_MODE_ATTITUDE - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); - mavlink_delay(500); + uint16_t store = 0; + SendDebugln(" Warming up ADC..."); + delay(500); // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! // ----------------------- demo_servos(2); - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); - mavlink_delay(1000); + SendDebugln(" Beginning IMU calibration; do not move plane"); + delay(1000); - imu.init(IMU::COLD_START, mavlink_delay); + imu.init(IMU::COLD_START); dcm.set_centripetal(1); // read Baro pressure at ground @@ -477,7 +479,7 @@ static void startup_IMU_ground(void) } -static void update_GPS_light(void) +void update_GPS_light(void) { // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- @@ -505,16 +507,15 @@ static void update_GPS_light(void) } -static void resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; +void resetPerfData(void) { + mainLoop_count = 0; + G_Dt_max = 0; dcm.gyro_sat_count = 0; imu.adc_constraints = 0; dcm.renorm_sqrt_count = 0; dcm.renorm_blowup_count = 0; - gps_fix_count = 0; - pmTest1 = 0; - perf_mon_timer = millis(); + gps_fix_count = 0; + perf_mon_timer = millis(); } @@ -525,7 +526,7 @@ static void resetPerfData(void) { * be larger than HP or you'll be in big trouble! The smaller the gap, the more * careful you need to be. Julian Gall 6 - Feb - 2009. */ -static unsigned long freeRAM() { +unsigned long freeRAM() { uint8_t * heapptr, * stackptr; stackptr = (uint8_t *)malloc(4); // use stackptr temporarily heapptr = stackptr; // save value of heap pointer @@ -534,20 +535,3 @@ static unsigned long freeRAM() { return stackptr - heapptr; } - -/* - map from a 8 bit EEPROM baud rate to a real baud rate - */ -static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) -{ - switch (rate) { - case 9: return 9600; - case 19: return 19200; - case 38: return 38400; - case 57: return 57600; - case 111: return 111100; - case 115: return 115200; - } - Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); - return default_baud; -} diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 926e95ca1a..9200012f40 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -1,6 +1,4 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CLI_ENABLED == ENABLED +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. @@ -28,7 +26,7 @@ static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details -static const struct Menu::command test_menu_commands[] PROGMEM = { +const struct Menu::command test_menu_commands[] PROGMEM = { {"pwm", test_radio_pwm}, {"radio", test_radio}, {"failsafe", test_failsafe}, @@ -66,15 +64,14 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { // A Macro to create the Menu MENU(test_menu, "test", test_menu_commands); -static int8_t +int8_t test_mode(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("Test Mode\n\n")); test_menu.run(); - return 0; } -static void print_hit_enter() +void print_hit_enter() { Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); } @@ -107,7 +104,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) // ---------------------------------------------------------- read_radio(); - Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), + Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.channel_roll.radio_in, g.channel_pitch.radio_in, g.channel_throttle.radio_in, @@ -150,7 +147,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) // write out the servo PWM values // ------------------------------ - set_servos(); + set_servos_4(); Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.channel_roll.control_in, @@ -258,7 +255,7 @@ if (g.battery_monitoring == 4) { // write out the servo PWM values // ------------------------------ - set_servos(); + set_servos_4(); if(Serial.available() > 0){ return (0); @@ -318,11 +315,11 @@ test_wp(uint8_t argc, const Menu::arg *argv) return (0); } -static void -test_wp_print(struct Location *cmd, byte wp_index) +void +test_wp_print(struct Location *cmd, byte index) { Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), - (int)wp_index, + (int)index, (int)cmd->id, (int)cmd->options, (int)cmd->p1, @@ -379,24 +376,20 @@ test_dipswitches(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - if (!g.switch_enable) { - Serial.println_P(PSTR("dip switches disabled, using EEPROM")); - } - while(1){ delay(100); update_servo_switches(); - if (g.mix_mode == 0) { + if (mix_mode == 0) { Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), - (int)g.channel_roll.get_reverse(), - (int)g.channel_pitch.get_reverse(), - (int)g.channel_rudder.get_reverse()); + (int)reverse_roll, + (int)reverse_pitch, + (int)reverse_rudder); } else { Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), - (int)g.reverse_elevons, - (int)g.reverse_ch1_elevon, - (int)g.reverse_ch2_elevon); + (int)reverse_elevons, + (int)reverse_ch1_elevon, + (int)reverse_ch2_elevon); } if(Serial.available() > 0){ return (0); @@ -532,29 +525,13 @@ test_gyro(uint8_t argc, const Menu::arg *argv) static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { - if (!g.compass_enabled) { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); - return (0); - } - - compass.set_orientation(MAG_ORIENTATION); - if (!compass.init()) { - Serial.println_P(PSTR("Compass initialisation failed!")); - return 0; - } - dcm.set_compass(&compass); - report_compass(); - - // we need the DCM initialised for this test - imu.init(IMU::COLD_START); - int counter = 0; + if(g.compass_enabled) { //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); - print_hit_enter(); + print_hit_enter(); - while(1) { + while(1){ delay(20); if (millis() - fast_loopTimer > 19) { delta_ms_fast_loop = millis() - fast_loopTimer; @@ -565,38 +542,40 @@ test_mag(uint8_t argc, const Menu::arg *argv) // --- dcm.update_DCM(G_Dt); - medium_loopCounter++; - if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - medium_loopCounter = 0; - } + if(g.compass_enabled) { + medium_loopCounter++; + if(medium_loopCounter == 5){ + compass.read(); // Read magnetometer + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + medium_loopCounter = 0; + } + } counter++; if (counter>20) { - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z, - maggy.x, - maggy.y, - maggy.z); - counter=0; - } - } - if (Serial.available() > 0) { - break; - } - } + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z, + maggy.x, + maggy.y, + maggy.z); + counter=0; +} - // save offsets. This allows you to get sane offset values using - // the CLI before you go flying. - Serial.println_P(PSTR("saving offsets")); - compass.save_offsets(); - return (0); + } + if(Serial.available() > 0){ + return (0); + } + } + } else { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } } #endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS @@ -609,11 +588,14 @@ test_mag(uint8_t argc, const Menu::arg *argv) static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv) { - unsigned airspeed_ch = adc.Ch(AIRSPEED_CH); // Serial.println(adc.Ch(AIRSPEED_CH)); - Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch); + if ((adc.Ch(AIRSPEED_CH) > 2000) && (adc.Ch(AIRSPEED_CH) < 2900)){ + airspeed_enabled = false; + }else{ + airspeed_enabled = true; + } - if (g.airspeed_enabled == false){ + if (airspeed_enabled == false){ Serial.printf_P(PSTR("airspeed: ")); print_enabled(false); return (0); @@ -632,8 +614,11 @@ test_airspeed(uint8_t argc, const Menu::arg *argv) if(Serial.available() > 0){ return (0); } + if(Serial.available() > 0){ + return (0); } } + } } @@ -684,5 +669,3 @@ test_rawgps(uint8_t argc, const Menu::arg *argv) } } #endif // HIL_MODE == HIL_MODE_DISABLED - -#endif // CLI_ENABLED From b9fad1262ac5b1c0df7ecf08280bc3e71558fa9d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:49:11 +1000 Subject: [PATCH 02/31] move Frame into ArduCopter --- .../Frame}/PD-PCB_BOTTOM-SOLDER.pdf | Bin {Frame => ArduCopter/Frame}/PD-PCB_BOTTOM_LAYER.pdf | Bin {Frame => ArduCopter/Frame}/PD-PCB_TOP_LAYER.pdf | Bin {Frame => ArduCopter/Frame}/PD-PCB_TOP_SOLDER.pdf | Bin {Frame => ArduCopter/Frame}/README | 0 .../Frame}/TradHeli/Trex450SonarMount.skp | Bin .../Frame}/TradHeli/apmTrex450MountV9.ai | 0 .../Frame}/TradHeli/apmTrex450MountV9.crv | Bin .../Frame}/TradHeli/apmTrex450MountV9.dxf | 0 .../Frame}/TradHeli/apmTrex450MountV9.eps | 0 {Frame => ArduCopter/Frame}/TradHeli/gpsMountV2.skp | Bin 11 files changed, 0 insertions(+), 0 deletions(-) rename {Frame => ArduCopter/Frame}/PD-PCB_BOTTOM-SOLDER.pdf (100%) rename {Frame => ArduCopter/Frame}/PD-PCB_BOTTOM_LAYER.pdf (100%) rename {Frame => ArduCopter/Frame}/PD-PCB_TOP_LAYER.pdf (100%) rename {Frame => ArduCopter/Frame}/PD-PCB_TOP_SOLDER.pdf (100%) rename {Frame => ArduCopter/Frame}/README (100%) rename {Frame => ArduCopter/Frame}/TradHeli/Trex450SonarMount.skp (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.ai (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.crv (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.dxf (100%) rename {Frame => ArduCopter/Frame}/TradHeli/apmTrex450MountV9.eps (100%) rename {Frame => ArduCopter/Frame}/TradHeli/gpsMountV2.skp (100%) diff --git a/Frame/PD-PCB_BOTTOM-SOLDER.pdf b/ArduCopter/Frame/PD-PCB_BOTTOM-SOLDER.pdf similarity index 100% rename from Frame/PD-PCB_BOTTOM-SOLDER.pdf rename to ArduCopter/Frame/PD-PCB_BOTTOM-SOLDER.pdf diff --git a/Frame/PD-PCB_BOTTOM_LAYER.pdf b/ArduCopter/Frame/PD-PCB_BOTTOM_LAYER.pdf similarity index 100% rename from Frame/PD-PCB_BOTTOM_LAYER.pdf rename to ArduCopter/Frame/PD-PCB_BOTTOM_LAYER.pdf diff --git a/Frame/PD-PCB_TOP_LAYER.pdf b/ArduCopter/Frame/PD-PCB_TOP_LAYER.pdf similarity index 100% rename from Frame/PD-PCB_TOP_LAYER.pdf rename to ArduCopter/Frame/PD-PCB_TOP_LAYER.pdf diff --git a/Frame/PD-PCB_TOP_SOLDER.pdf b/ArduCopter/Frame/PD-PCB_TOP_SOLDER.pdf similarity index 100% rename from Frame/PD-PCB_TOP_SOLDER.pdf rename to ArduCopter/Frame/PD-PCB_TOP_SOLDER.pdf diff --git a/Frame/README b/ArduCopter/Frame/README similarity index 100% rename from Frame/README rename to ArduCopter/Frame/README diff --git a/Frame/TradHeli/Trex450SonarMount.skp b/ArduCopter/Frame/TradHeli/Trex450SonarMount.skp similarity index 100% rename from Frame/TradHeli/Trex450SonarMount.skp rename to ArduCopter/Frame/TradHeli/Trex450SonarMount.skp diff --git a/Frame/TradHeli/apmTrex450MountV9.ai b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.ai similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.ai rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.ai diff --git a/Frame/TradHeli/apmTrex450MountV9.crv b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.crv similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.crv rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.crv diff --git a/Frame/TradHeli/apmTrex450MountV9.dxf b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.dxf similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.dxf rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.dxf diff --git a/Frame/TradHeli/apmTrex450MountV9.eps b/ArduCopter/Frame/TradHeli/apmTrex450MountV9.eps similarity index 100% rename from Frame/TradHeli/apmTrex450MountV9.eps rename to ArduCopter/Frame/TradHeli/apmTrex450MountV9.eps diff --git a/Frame/TradHeli/gpsMountV2.skp b/ArduCopter/Frame/TradHeli/gpsMountV2.skp similarity index 100% rename from Frame/TradHeli/gpsMountV2.skp rename to ArduCopter/Frame/TradHeli/gpsMountV2.skp From ec398505a6409959a220d7b76d5d1c025a5f6136 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 11:51:21 +1000 Subject: [PATCH 03/31] rename main pde file for arduino requirements --- ArduCopter/{ArduCopterMega.pde => ArduCopter.pde} | 0 ArduPlane/{ArduPilotMega.pde => ArduPlane.pde} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename ArduCopter/{ArduCopterMega.pde => ArduCopter.pde} (100%) rename ArduPlane/{ArduPilotMega.pde => ArduPlane.pde} (100%) diff --git a/ArduCopter/ArduCopterMega.pde b/ArduCopter/ArduCopter.pde similarity index 100% rename from ArduCopter/ArduCopterMega.pde rename to ArduCopter/ArduCopter.pde diff --git a/ArduPlane/ArduPilotMega.pde b/ArduPlane/ArduPlane.pde similarity index 100% rename from ArduPlane/ArduPilotMega.pde rename to ArduPlane/ArduPlane.pde From f40c85a6012524fdee73d9ae6dd87972531886ca Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Fri, 9 Sep 2011 16:02:22 +0200 Subject: [PATCH 04/31] This is the real HEAD of the APM_Camera branch. Seams that lots of changes got lost in the SVN to GIT port --- ArduPlane/Cam_move.pde | 210 ------------- ArduPlane/Cam_trigger.pde | 41 --- libraries/AP_Camera/AP_Camera.cpp | 116 ++++++++ libraries/AP_Camera/{Camera.h => AP_Camera.h} | 34 +-- libraries/AP_Camera/Camera.cpp | 277 ------------------ 5 files changed, 126 insertions(+), 552 deletions(-) delete mode 100644 ArduPlane/Cam_move.pde delete mode 100644 ArduPlane/Cam_trigger.pde create mode 100644 libraries/AP_Camera/AP_Camera.cpp rename libraries/AP_Camera/{Camera.h => AP_Camera.h} (63%) delete mode 100644 libraries/AP_Camera/Camera.cpp diff --git a/ArduPlane/Cam_move.pde b/ArduPlane/Cam_move.pde deleted file mode 100644 index bc8ad0eac7..0000000000 --- a/ArduPlane/Cam_move.pde +++ /dev/null @@ -1,210 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#if CAMERA == ENABLED - -void init_camera() -{ - g.rc_camera_pitch.set_angle(4500); // throw of servo? - g.rc_camera_pitch.radio_min = 1000; // lowest radio input - g.rc_camera_pitch.radio_trim = 1500; // middle radio input - g.rc_camera_pitch.radio_max = 2000; // highest radio input - - g.rc_camera_roll.set_angle(4500); - g.rc_camera_roll.radio_min = 1000; - g.rc_camera_roll.radio_trim = 1500; - g.rc_camera_roll.radio_max = 2000; - - - //use test target for now - camera_target = home; - -} - -void camera() -{ - //decide what happens to camera depending on camera mode - switch(camera_mode) - { - case 0: - //do nothing, i.e lock camera in place - break; - case 1: - //stabilize - target_vector.x=0; //east to west gives +tive value (i.e. longitude) - target_vector.y=0; //south to north gives +tive value (i.e. latitude) - target_vector.z=100; //downwards is +tive - camera_move(); - break; - case 2: - //track target - if(g_gps->fix) - { - target_vector=get_location_vector(¤t_loc,&camera_target); - camera_move(); - } - break; - } -} - -void camera_move() -{ - Matrix3f m = dcm.get_dcm_transposed(); - Vector3 targ = m*target_vector; //to do: find out notion of x y convention - - switch(gimbal_mode) - { - case 0: // pitch & roll - cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch - cam_roll = degrees(atan2(targ.y, targ.z)); //roll - // check_limits(pitch); - // check_limits(roll); - // camera_out(); - break; - - case 1: // pitch and yaw - cam_tilt = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; - cam_pan = 9000 + atan2(-targ.y, targ.x) * 5729.57795; - if (cam_pan < 0) cam_pan += 36000; - // check_limits(pitch); - // check_limits(yaw); - // camera_out(); - break; - - /* case 2: // pitch, roll & yaw - not started - float cam_ritch = 100; - float cam_yoll = 100; - float cam_paw = 100; - break; */ - } -} - -/* void check_limits(axis,variable) // Use servo definitions to calculate for all servo throws - TO DO -{ - // find limits of servo range in deg - track_pan_right = PAN_CENTER + (PAN_RANGE/2); - track_pan_left = track_pan_right + (360 - PAN_RANGE); - if (track_pan_left > 360){ - track_pan_left = track_pan_left - 360; - } - // check track_bearing is "safe" - not outside pan servo limits - // if the bearing lies in the servo dead zone change bearing to closet edge - if (track_bearing < track_pan_left && track_bearing > track_pan_right){ - track_oor_l = abs(track_bearing - track_pan_left); - track_oor_r = abs(track_bearing - track_pan_right); - if (track_oor_r > track_oor_l){ - track_bearing = track_pan_right; - } - if (track_oor_l > track_oor_r){ - track_bearing = track_pan_left; - } - } - // center bearing to cam_servo center - track_pan_deg = track_bearing - PAN_CENTER; - // make negative is left rotation - if (track_pan_deg > 180){ - track_pan_deg = (180 - (track_pan_deg - 180)) * -1; - } - -} */ - -void camera_out() -{ - switch(gimbal_mode) - { - case 0: // pitch & roll - g.rc_camera_pitch.servo_out = cam_pitch; - g.rc_camera_pitch.calc_pwm(); - g.rc_camera_roll.servo_out = cam_roll; - g.rc_camera_roll.calc_pwm(); - break; - - case 1: // pitch and yaw - g.rc_camera_pitch.servo_out = cam_tilt; - g.rc_camera_pitch.calc_pwm(); - g.rc_camera_roll.servo_out = cam_pan; // borrowing roll servo output for pan/yaw - g.rc_camera_roll.calc_pwm(); - break; - - /*case 2: // pitch, roll & yaw - g.rc_camera_pitch.servo_out = cam_ritch; - g.rc_camera_pitch.calc_pwm(); - - g.rc_camera_roll.servo_out = cam_yoll; - g.rc_camera_roll.calc_pwm(); - - g.rc_camera_yaw.servo_out = cam_paw; // camera_yaw doesn't exist it should unless we use another channel - g.rc_camera_yaw.calc_pwm(); - break; */ - } -#if defined PITCH_SERVO - APM_RC.OutputCh(PITCH_SERVO, g.rc_camera_pitch.radio_out); -#endif -#if defined ROLL_SERVO - APM_RC.OutputCh(ROLL_SERVO, g.rc_camera_roll.radio_out); -#endif -/*#if defined YAW_SERVO - APM_RC.OutputCh(YAW_SERVO, g.rc_camera_yaw.radio_out); -#endif */ - -#if CAM_DEBUG == ENABLED - //for debugging purposes - Serial.println(); - Serial.print("current_loc: lat: "); - Serial.print(current_loc.lat); - Serial.print(", lng: "); - Serial.print(current_loc.lng); - Serial.print(", alt: "); - Serial.print(current_loc.alt); - Serial.println(); - Serial.print("target_loc: lat: "); - Serial.print(camera_target.lat); - Serial.print(", lng: "); - Serial.print(camera_target.lng); - Serial.print(", alt: "); - Serial.print(camera_target.alt); - Serial.print(", distance: "); - Serial.print(get_distance(¤t_loc,&camera_target)); - Serial.print(", bearing: "); - Serial.print(get_bearing(¤t_loc,&camera_target)); - Serial.println(); - Serial.print("dcm_angles: roll: "); - Serial.print(degrees(dcm.roll)); - Serial.print(", pitch: "); - Serial.print(degrees(dcm.pitch)); - Serial.print(", yaw: "); - Serial.print(degrees(dcm.yaw)); - Serial.println(); - Serial.print("target_vector: x: "); - Serial.print(target_vector.x,2); - Serial.print(", y: "); - Serial.print(target_vector.y,2); - Serial.print(", z: "); - Serial.print(target_vector.z,2); - Serial.println(); - Serial.print("rotated_target_vector: x: "); - Serial.print(targ.x,2); - Serial.print(", y: "); - Serial.print(targ.y,2); - Serial.print(", z: "); - Serial.print(targ.z,2); - Serial.println(); - Serial.print("gimbal type 0: roll: "); - Serial.print(roll); - Serial.print(", pitch: "); - Serial.print(pitch); - Serial.println(); - /* Serial.print("gimbal type 1: pitch: "); - Serial.print(pan); - Serial.print(", roll: "); - Serial.print(tilt); - Serial.println(); */ - /* Serial.print("gimbal type 2: pitch: "); - Serial.print(ritch); - Serial.print(", roll: "); - Serial.print(yoll); - Serial.print(", yaw: "); - Serial.print(paw); - Serial.println(); */ -#endif -} -#endif diff --git a/ArduPlane/Cam_trigger.pde b/ArduPlane/Cam_trigger.pde deleted file mode 100644 index 7b03a90c8b..0000000000 --- a/ArduPlane/Cam_trigger.pde +++ /dev/null @@ -1,41 +0,0 @@ -void servo_pic() // Servo operated camera -{ - APM_RC.OutputCh(CAM_SERVO,1500 + (333)); // Camera click, not enough - add more, wring way - put a minus before bracket number (-300) - delay(250); // delay - APM_RC.OutputCh(CAM_SERVO,1500); // Return servo to mid position -} - -void relay_picture() // basic relay activation -{ - relay_on(); - delay(250); // 0.25 seconds delay - relay_off(); -} - -void throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic = 10){ - servo_pic(); // triggering method - thr_pic = 0; - g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method - g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void NPN_trigger() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // To Do: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - delay(50); - digitalWrite(camtrig, LOW); -} diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp new file mode 100644 index 0000000000..df32a4adac --- /dev/null +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -0,0 +1,116 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#include + +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function +extern long wp_distance; +extern "C" { +void relay_on(); +void relay_off(); +} + +void +AP_Camera::servo_pic() // Servo operated camera +{ + if (g_rc_function[RC_Channel_aux::k_cam_trigger]) + { + g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_max; + keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles + } +} + +void +AP_Camera::relay_pic() // basic relay activation +{ + relay_on(); + keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles +} + +void +AP_Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. +{ +// TODO find a way to do this without using the global parameter g +// g.channel_throttle.radio_out = g.throttle_min; + if (thr_pic == 10){ + servo_pic(); // triggering method + thr_pic = 0; +// g.channel_throttle.radio_out = g.throttle_cruise; + } + thr_pic++; +} + +void +AP_Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. +{ +// TODO find a way to do this without using the global parameter g +// g.channel_throttle.radio_out = g.throttle_min; + if (wp_distance < 3){ + servo_pic(); // triggering method +// g.channel_throttle.radio_out = g.throttle_cruise; + } +} + +void +AP_Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. +{ + // TODO: Assign pin spare pin for output + digitalWrite(camtrig, HIGH); + keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles +} + +// single entry point to take pictures +void +AP_Camera::trigger_pic() +{ + switch (trigger_type) + { + case 0: + servo_pic(); // Servo operated camera + break; + case 1: + relay_pic(); // basic relay activation + break; + case 2: + throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. + break; + case 3: + distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. + break; + case 4: + NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. + break; + } +} + +// de-activate the trigger after some delay, but without using a delay() function +void +AP_Camera::trigger_pic_cleanup() +{ + if (keep_cam_trigg_active_cycles) + { + keep_cam_trigg_active_cycles --; + } + else + { + switch (trigger_type) + { + case 0: + case 2: + case 3: + if (g_rc_function[RC_Channel_aux::k_cam_trigger]) + { + g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; + } + break; + case 1: + // TODO for some strange reason the function call bellow gives a linker error + //relay_off(); + PORTL &= ~B00000100; // hardcoded version of relay_off(). Replace with a proper function call later. + break; + case 4: + digitalWrite(camtrig, LOW); + break; + } + } +} diff --git a/libraries/AP_Camera/Camera.h b/libraries/AP_Camera/AP_Camera.h similarity index 63% rename from libraries/AP_Camera/Camera.h rename to libraries/AP_Camera/AP_Camera.h index 88dc792748..e1c520131a 100644 --- a/libraries/AP_Camera/Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -1,16 +1,16 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -/// @file Camera.h +/// @file AP_Camera.h /// @brief Photo or video camera manager, with EEPROM-backed storage of constants. -#ifndef CAMERA_H -#define CAMERA_H +#ifndef AP_CAMERA_H +#define AP_CAMERA_H #include /// @class Camera /// @brief Object managing a Photo or video camera -class Camera{ +class AP_Camera{ protected: AP_Var_group _group; // must be before all vars to keep ctor init order correct @@ -20,45 +20,31 @@ public: /// @param key EEPROM storage key for the camera configuration parameters. /// @param name Optional name for the group. /// - Camera(AP_Var::Key key, const prog_char_t *name) : + AP_Camera(AP_Var::Key key, const prog_char_t *name) : _group(key, name), - mode (&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name - trigger_type(&_group, 1, 0, name ? PSTR("TRIGGER_MODE") : 0), + trigger_type(&_group, 0, 0, name ? PSTR("TRIGG_TYPE") : 0), picture_time (0), // waypoint trigger variable thr_pic (0), // timer variable for throttle_pic camtrig (83), // PK6 chosen as it not near anything so safer for soldering -// camera_target (home), // use test target for now - gimbal_type (1), - keep_cam_trigg_active_cycles (0) + keep_cam_trigg_active_cycles (0), + wp_distance_min (10) {} - // move the camera depending on the camera mode - void move(); - // single entry point to take pictures void trigger_pic(); // de-activate the trigger after some delay, but without using a delay() function void trigger_pic_cleanup(); - // call this from time to time to make sure the correct gimbal type gets choosen - void update_camera_gimbal_type(); - - // set camera orientation target - void set_target(struct Location target); - int picture_time; // waypoint trigger variable + long wp_distance_min; // take picture if distance to WP is smaller than this private: uint8_t keep_cam_trigg_active_cycles; // event loop cycles to keep trigger active - struct Location camera_target; // point of interest for the camera to track -// struct Location GPS_mark; // GPS POI for position based triggering int thr_pic; // timer variable for throttle_pic int camtrig; // PK6 chosen as it not near anything so safer for soldering - AP_Int8 mode; // 0=do nothing, 1=stabilize, 2=track target, 3=manual, 4=simple stabilize test AP_Int8 trigger_type; // 0=Servo, 1=relay, 2=throttle_off time, 3=throttle_off waypoint 4=transistor - uint8_t gimbal_type; // 0=pitch & roll (tilt & roll), 1=yaw & pitch(pan & tilt), 2=pitch, roll & yaw (to be added) void servo_pic(); // Servo operated camera void relay_pic(); // basic relay activation @@ -68,4 +54,4 @@ private: }; -#endif /* CAMERA_H */ +#endif /* AP_CAMERA_H */ diff --git a/libraries/AP_Camera/Camera.cpp b/libraries/AP_Camera/Camera.cpp deleted file mode 100644 index 2c9bb928fd..0000000000 --- a/libraries/AP_Camera/Camera.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -#include "Camera.h" -#include "../RC_Channel/RC_Channel.h" - -void -Camera::move() -{ - Vector3 target_vector(0,0,1); // x, y, z to target before rotating to planes axis, values are in meters - - //decide what happens to camera depending on camera mode - switch(mode) - { - case 0: - //do nothing, i.e lock camera in place - return; - break; - case 1: - //stabilize - target_vector.x=0; //east to west gives +tive value (i.e. longitude) - target_vector.y=0; //south to north gives +tive value (i.e. latitude) - target_vector.z=100; //downwards is +tive - break; - case 2: - //track target - if(g_gps->fix) - { - target_vector=get_location_vector(¤t_loc,&camera_target); - } - break; - case 3: // radio manual control - case 4: // test (level the camera and point north) - break; // see code 25 lines bellow - } - - Matrix3f m = dcm.get_dcm_transposed(); - Vector3 targ = m*target_vector; //to do: find out notion of x y convention - switch(gimbal_type) - { - case 0: // pitch & roll (tilt & roll) - cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch - cam_roll = degrees(atan2(targ.y, targ.z)); //roll - break; - - case 1: // yaw & pitch (pan & tilt) - cam_pitch = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1; - cam_yaw = 9000 + atan2(-targ.y, targ.x) * 5729.57795; - break; - -/* case 2: // pitch, roll & yaw - not started - cam_ritch = 0; - cam_yoll = 0; - cam_paw = 0; - break; */ - - } - - //some camera modes overwrite the gimbal_type calculations - switch(mode) - { - case 3: // radio manual control - if (rc_function[CAM_PITCH]) - cam_pitch = map(rc_function[CAM_PITCH]->radio_in, - rc_function[CAM_PITCH]->radio_min, - rc_function[CAM_PITCH]->radio_max, - rc_function[CAM_PITCH]->angle_min, - rc_function[CAM_PITCH]->radio_max); - if (rc_function[CAM_ROLL]) - cam_roll = map(rc_function[CAM_ROLL]->radio_in, - rc_function[CAM_ROLL]->radio_min, - rc_function[CAM_ROLL]->radio_max, - rc_function[CAM_ROLL]->angle_min, - rc_function[CAM_ROLL]->radio_max); - if (rc_function[CAM_YAW]) - cam_yaw = map(rc_function[CAM_YAW]->radio_in, - rc_function[CAM_YAW]->radio_min, - rc_function[CAM_YAW]->radio_max, - rc_function[CAM_YAW]->angle_min, - rc_function[CAM_YAW]->radio_max); - break; - case 4: // test (level the camera and point north) - cam_pitch = -dcm.pitch_sensor; - cam_yaw = dcm.yaw_sensor; // do not invert because the servo is mounted upside-down on my system - // TODO: the "trunk" code can invert using parameters, but this branch still can't - cam_roll = -dcm.roll_sensor; - break; - } - - #if CAM_DEBUG == ENABLED - //for debugging purposes - Serial.println(); - Serial.print("current_loc: lat: "); - Serial.print(current_loc.lat); - Serial.print(", lng: "); - Serial.print(current_loc.lng); - Serial.print(", alt: "); - Serial.print(current_loc.alt); - Serial.println(); - Serial.print("target_loc: lat: "); - Serial.print(camera_target.lat); - Serial.print(", lng: "); - Serial.print(camera_target.lng); - Serial.print(", alt: "); - Serial.print(camera_target.alt); - Serial.print(", distance: "); - Serial.print(get_distance(¤t_loc,&camera_target)); - Serial.print(", bearing: "); - Serial.print(get_bearing(¤t_loc,&camera_target)); - Serial.println(); - Serial.print("dcm_angles: roll: "); - Serial.print(degrees(dcm.roll)); - Serial.print(", pitch: "); - Serial.print(degrees(dcm.pitch)); - Serial.print(", yaw: "); - Serial.print(degrees(dcm.yaw)); - Serial.println(); - Serial.print("target_vector: x: "); - Serial.print(target_vector.x,2); - Serial.print(", y: "); - Serial.print(target_vector.y,2); - Serial.print(", z: "); - Serial.print(target_vector.z,2); - Serial.println(); - Serial.print("rotated_target_vector: x: "); - Serial.print(targ.x,2); - Serial.print(", y: "); - Serial.print(targ.y,2); - Serial.print(", z: "); - Serial.print(targ.z,2); - Serial.println(); - Serial.print("gimbal type 0: roll: "); - Serial.print(roll); - Serial.print(", pitch: "); - Serial.print(pitch); - Serial.println(); - /* Serial.print("gimbal type 1: pitch: "); - Serial.print(pan); - Serial.print(", roll: "); - Serial.print(tilt); - Serial.println(); */ - /* Serial.print("gimbal type 2: pitch: "); - Serial.print(ritch); - Serial.print(", roll: "); - Serial.print(yoll); - Serial.print(", yaw: "); - Serial.print(paw); - Serial.println(); */ -#endif -} - - -void -Camera::set_target(struct Location target) -{ - camera_target = target; -} - - -void -Camera::update_camera_gimbal_type() -{ - - // Auto detect the camera gimbal type depending on the functions assigned to the servos - if ((rc_function[CAM_YAW] == NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL)) - { - gimbal_type = 0; - } - if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] == NULL)) - { - gimbal_type = 1; - } - if ((rc_function[CAM_YAW] != NULL) && (rc_function[CAM_PITCH] != NULL) && (rc_function[CAM_ROLL] != NULL)) - { - gimbal_type = 2; - } -} - -void -Camera::servo_pic() // Servo operated camera -{ - if (rc_function[CAM_TRIGGER]) - { - cam_trigger = rc_function[CAM_TRIGGER]->radio_max; - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles - } -} - -void -Camera::relay_pic() // basic relay activation -{ - relay_on(); - keep_cam_trigg_active_cycles = 2; // leave a message that it should be active for two event loop cycles -} - -void -Camera::throttle_pic() // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (thr_pic == 10){ - servo_pic(); // triggering method - thr_pic = 0; - g.channel_throttle.radio_out = g.throttle_cruise; - } - thr_pic++; -} - -void -Camera::distance_pic() // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. -{ - g.channel_throttle.radio_out = g.throttle_min; - if (wp_distance < 3){ - servo_pic(); // triggering method - g.channel_throttle.radio_out = g.throttle_cruise; - } -} - -void -Camera::NPN_pic() // hacked the circuit to run a transistor? use this trigger to send output. -{ - // To Do: Assign pin spare pin for output - digitalWrite(camtrig, HIGH); - keep_cam_trigg_active_cycles = 1; // leave a message that it should be active for two event loop cycles -} - -// single entry point to take pictures -void -Camera::trigger_pic() -{ - switch (trigger_type) - { - case 0: - servo_pic(); // Servo operated camera - break; - case 1: - relay_pic(); // basic relay activation - break; - case 2: - throttle_pic(); // pictures blurry? use this trigger. Turns off the throttle until for # of cycles of medium loop then takes the picture and re-enables the throttle. - break; - case 3: - distance_pic(); // pictures blurry? use this trigger. Turns off the throttle until closer to waypoint then takes the picture and re-enables the throttle. - break; - case 4: - NPN_pic(); // hacked the circuit to run a transistor? use this trigger to send output. - break; - } -} - -// de-activate the trigger after some delay, but without using a delay() function -void -Camera::trigger_pic_cleanup() -{ - if (keep_cam_trigg_active_cycles) - { - keep_cam_trigg_active_cycles --; - } - else - { - switch (trigger_type) - { - case 0: - case 2: - case 3: - if (rc_function[CAM_TRIGGER]) - { - cam_trigger = rc_function[CAM_TRIGGER]->radio_min; - } - break; - case 1: - relay_off(); - break; - case 4: - digitalWrite(camtrig, LOW); - break; - } - } -} From b7a0d8836adc7c9c856d3e8966b5d0a98b099be7 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Fri, 9 Sep 2011 16:18:38 +0200 Subject: [PATCH 05/31] 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 f4998c36734aa12470962ef48bf324a6330710d9 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 13:26:29 +0200 Subject: [PATCH 06/31] Moved a function from radio.pde to the RC_Channel_aux library. Now its more readable and reusable --- ArduPlane/Attitude.pde | 8 ++++---- ArduPlane/radio.pde | 28 --------------------------- libraries/RC_Channel/RC_Channel.cpp | 30 +++++++++++++++++++++++++++++ libraries/RC_Channel/RC_Channel.h | 3 +++ 4 files changed, 37 insertions(+), 32 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index e5a109744d..07300a65b6 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -339,10 +339,10 @@ static void set_servos(void) APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos // Route configurable aux. functions to their respective servos - aux_servo_out(&g.rc_5, CH_5); - aux_servo_out(&g.rc_6, CH_6); - aux_servo_out(&g.rc_7, CH_7); - aux_servo_out(&g.rc_8, CH_8); + g.rc_5.output_ch(CH_5); + g.rc_6.output_ch(CH_6); + g.rc_7.output_ch(CH_7); + g.rc_8.output_ch(CH_8); #endif } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index dd346d8f91..f38d57cf1f 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -247,34 +247,6 @@ static void trim_radio() if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } -// map a function to a servo channel -static void aux_servo_out(RC_Channel_aux* ch, unsigned char ch_nr) -{ - switch(ch->function) - { - case RC_Channel_aux::k_none: // disabled - return; - break; - case RC_Channel_aux::k_mount_yaw: // mount yaw (pan) - case RC_Channel_aux::k_mount_pitch: // mount pitch (tilt) - case RC_Channel_aux::k_mount_roll: // mount roll - case RC_Channel_aux::k_cam_trigger: // camera trigger - case RC_Channel_aux::k_cam_open: // camera open - case RC_Channel_aux::k_flap: // flaps - case RC_Channel_aux::k_flap_auto: // flaps automated - case RC_Channel_aux::k_aileron: // aileron - case RC_Channel_aux::k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) - case RC_Channel_aux::k_egg_drop: // egg drop - case RC_Channel_aux::k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning - break; - case RC_Channel_aux::k_manual: // manual - ch->radio_out = ch->radio_in; - break; - } - - APM_RC.OutputCh(ch_nr, ch->radio_out); -} - // update the g_rc_function array from pointers to rc_x channels static void update_aux_servo_function() { diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index d5cf7105fa..2673f26e53 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -11,6 +11,7 @@ #include #include +#include #include "WProgram.h" #include "RC_Channel.h" @@ -253,3 +254,32 @@ RC_Channel_aux::closest_limit(int16_t angle) return angle; } + +// map a function to a servo channel and output it +void +RC_Channel_aux::output_ch(unsigned char ch_nr) +{ + switch(function) + { + case k_none: // disabled + return; + break; + case k_mount_yaw: // mount yaw (pan) + case k_mount_pitch: // mount pitch (tilt) + case k_mount_roll: // mount roll + case k_cam_trigger: // camera trigger + case k_cam_open: // camera open + case k_flap: // flaps + case k_flap_auto: // flaps automated + case k_aileron: // aileron + case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) + case k_egg_drop: // egg drop + case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning + break; + case k_manual: // manual + radio_out = radio_in; + break; + } + + APM_RC.OutputCh(ch_nr, radio_out); +} diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 58557d9dda..1519611c4b 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -144,6 +144,9 @@ public: AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval + + void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it + }; #endif From 59cbb8ec6f5391dca1ffabdae35c2992bbc325f1 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 13:47:09 +0200 Subject: [PATCH 07/31] Moved a camera mount function call, out of the servos update function --- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/radio.pde | 4 ---- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 2b744873a2..45b95b69ee 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -730,6 +730,9 @@ static void slow_loop() update_aux_servo_function(); +#if CAMERA == ENABLED + camera_mount.update_mount_type(); +#endif break; case 2: diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index f38d57cf1f..11513fba41 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -267,8 +267,4 @@ static void update_aux_servo_function() g_rc_function[aux_servo_function[CH_6]] = &g.rc_6; g_rc_function[aux_servo_function[CH_7]] = &g.rc_7; g_rc_function[aux_servo_function[CH_8]] = &g.rc_8; - -#if CAMERA == ENABLED - camera_mount.update_mount_type(); -#endif } From 95af8dc1722048e05ad55a60a60078cf964a3744 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:06:46 +1000 Subject: [PATCH 08/31] mavlink: import new version with AP_ADC packet this adds the AP_ADC packet which gives us raw ADC values --- .../include/ardupilotmega/ardupilotmega.h | 7 +- .../ardupilotmega/mavlink_msg_ap_adc.h | 254 ++++++++++++++++++ .../include/ardupilotmega/testsuite.h | 54 ++++ .../include/ardupilotmega/version.h | 2 +- .../GCS_MAVLink/include/common/version.h | 2 +- .../message_definitions/ardupilotmega.xml | 10 + .../GCS_MAVLink/message_definitions/test.xml | 31 +++ 7 files changed, 355 insertions(+), 5 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h create mode 100644 libraries/GCS_MAVLink/message_definitions/test.xml diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index 1272728607..e650159666 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -48,6 +48,7 @@ extern "C" { #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" #include "./mavlink_msg_meminfo.h" +#include "./mavlink_msg_ap_adc.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h new file mode 100644 index 0000000000..d42edb15d3 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h @@ -0,0 +1,254 @@ +// MESSAGE AP_ADC PACKING + +#define MAVLINK_MSG_ID_AP_ADC 153 + +typedef struct __mavlink_ap_adc_t +{ + uint16_t adc1; ///< ADC output 1 + uint16_t adc2; ///< ADC output 2 + uint16_t adc3; ///< ADC output 3 + uint16_t adc4; ///< ADC output 4 + uint16_t adc5; ///< ADC output 5 + uint16_t adc6; ///< ADC output 6 +} mavlink_ap_adc_t; + +#define MAVLINK_MSG_ID_AP_ADC_LEN 12 +#define MAVLINK_MSG_ID_153_LEN 12 + + + +#define MAVLINK_MESSAGE_INFO_AP_ADC { \ + "AP_ADC", \ + 6, \ + { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ + { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ + { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ + { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ + { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ + { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ + } \ +} + + +/** + * @brief Pack a ap_adc message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; + return mavlink_finalize_message(msg, system_id, component_id, 12); +} + +/** + * @brief Pack a ap_adc message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_AP_ADC; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12); +} + +/** + * @brief Encode a ap_adc struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + +/** + * @brief Send a ap_adc message + * @param chan MAVLink channel to send the message + * + * @param adc1 ADC output 1 + * @param adc2 ADC output 2 + * @param adc3 ADC output 3 + * @param adc4 ADC output 4 + * @param adc5 ADC output 5 + * @param adc6 ADC output 6 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12); +#else + mavlink_ap_adc_t packet; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.adc5 = adc5; + packet.adc6 = adc6; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12); +#endif +} + +#endif + +// MESSAGE AP_ADC UNPACKING + + +/** + * @brief Get field adc1 from ap_adc message + * + * @return ADC output 1 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field adc2 from ap_adc message + * + * @return ADC output 2 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field adc3 from ap_adc message + * + * @return ADC output 3 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field adc4 from ap_adc message + * + * @return ADC output 4 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field adc5 from ap_adc message + * + * @return ADC output 5 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field adc6 from ap_adc message + * + * @return ADC output 6 + */ +static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Decode a ap_adc message into a struct + * + * @param msg The message to decode + * @param ap_adc C-struct to decode the message contents into + */ +static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) +{ +#if MAVLINK_NEED_BYTE_SWAP + ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); + ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); + ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); + ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); + ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); + ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); +#else + memcpy(ap_adc, _MAV_PAYLOAD(msg), 12); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index cd59317f8f..6a2e90cae5 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -185,11 +185,65 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ap_adc_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 17651, + 17755, + }; + mavlink_ap_adc_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.adc1 = packet_in.adc1; + packet1.adc2 = packet_in.adc2; + packet1.adc3 = packet_in.adc3; + packet1.adc4 = packet_in.adc4; + packet1.adc5 = packet_in.adc5; + packet1.adc6 = packet_in.adc6; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); + mavlink_msg_ap_adc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ifree memory + + raw ADC output + ADC output 1 + ADC output 2 + ADC output 3 + ADC output 4 + ADC output 5 + ADC output 6 + + diff --git a/libraries/GCS_MAVLink/message_definitions/test.xml b/libraries/GCS_MAVLink/message_definitions/test.xml new file mode 100644 index 0000000000..43a11e3d13 --- /dev/null +++ b/libraries/GCS_MAVLink/message_definitions/test.xml @@ -0,0 +1,31 @@ + + + 3 + + + Test all field types + char + string + uint8_t + uint16_t + uint32_t + uint64_t + int8_t + int16_t + int32_t + int64_t + float + double + uint8_t_array + uint16_t_array + uint32_t_array + uint64_t_array + int8_t_array + int16_t_array + int32_t_array + int64_t_array + float_array + double_array + + + From c65d038ccfbb52bf538f87e91ab8bde9df8500ff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 19:08:18 +1000 Subject: [PATCH 09/31] 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 0a793a13274abd4b41fb7037b96535a5af6b48be Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Sep 2011 23:24:50 +1000 Subject: [PATCH 10/31] Revert "added support for AP_ADC MAVLink packet" This reverts commit a65a2dda2c1bfa95437880227a3abddc95b329f5. I am reverting this not for any sane reason, but because my 3 APM boards now lock up on any I2C operation, and I don't know why. I can't see how this change could have caused it, but I don't want to take the chance. --- ArduPlane/GCS_Mavlink.pde | 3 +- ArduPlane/Mavlink_Common.h | 16 +- ArduPlane/defines.h | 1 - .../include/ardupilotmega/ardupilotmega.h | 7 +- .../ardupilotmega/mavlink_msg_ap_adc.h | 254 ------------------ .../include/ardupilotmega/testsuite.h | 54 ---- .../include/ardupilotmega/version.h | 2 +- .../GCS_MAVLink/include/common/version.h | 2 +- .../message_definitions/ardupilotmega.xml | 10 - .../GCS_MAVLink/message_definitions/test.xml | 31 --- 10 files changed, 8 insertions(+), 372 deletions(-) delete mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h delete mode 100644 libraries/GCS_MAVLink/message_definitions/test.xml diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index c1546d95a4..db34c15389 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -131,7 +131,8 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - send_message(MSG_RAW_ADC); + // Available datastream + //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 4c2147b2c4..3de4438cc2 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -344,20 +344,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; } - - case MSG_RAW_ADC: - { - CHECK_PAYLOAD_SIZE(AP_ADC); - mavlink_msg_ap_adc_send(chan, - analogRead(BATTERY_PIN1), - analogRead(BATTERY_PIN2), - analogRead(BATTERY_PIN3), - analogRead(BATTERY_PIN4), - analogRead(AN4), - analogRead(AN5)); - break; - } - default: break; } @@ -365,7 +351,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } -#define MAX_DEFERRED_MESSAGES 18 // should be at least equal to number of +#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of // switch types in mavlink_try_send_message() static struct mavlink_queue { uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a996ec0f48..fff75fda4f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -162,7 +162,6 @@ #define MSG_GPS_RAW 0x64 #define MSG_SERVO_OUT 0x70 -#define MSG_RAW_ADC 0x71 #define MSG_PIN_REQUEST 0x80 #define MSG_PIN_SET 0x81 diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index e650159666..1272728607 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -48,7 +48,6 @@ extern "C" { #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" #include "./mavlink_msg_meminfo.h" -#include "./mavlink_msg_ap_adc.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h deleted file mode 100644 index d42edb15d3..0000000000 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ap_adc.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE AP_ADC PACKING - -#define MAVLINK_MSG_ID_AP_ADC 153 - -typedef struct __mavlink_ap_adc_t -{ - uint16_t adc1; ///< ADC output 1 - uint16_t adc2; ///< ADC output 2 - uint16_t adc3; ///< ADC output 3 - uint16_t adc4; ///< ADC output 4 - uint16_t adc5; ///< ADC output 5 - uint16_t adc6; ///< ADC output 6 -} mavlink_ap_adc_t; - -#define MAVLINK_MSG_ID_AP_ADC_LEN 12 -#define MAVLINK_MSG_ID_153_LEN 12 - - - -#define MAVLINK_MESSAGE_INFO_AP_ADC { \ - "AP_ADC", \ - 6, \ - { { "adc1", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_ap_adc_t, adc1) }, \ - { "adc2", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_ap_adc_t, adc2) }, \ - { "adc3", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_ap_adc_t, adc3) }, \ - { "adc4", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_ap_adc_t, adc4) }, \ - { "adc5", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_ap_adc_t, adc5) }, \ - { "adc6", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_ap_adc_t, adc6) }, \ - } \ -} - - -/** - * @brief Pack a ap_adc message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; - return mavlink_finalize_message(msg, system_id, component_id, 12); -} - -/** - * @brief Pack a ap_adc message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t adc5,uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_AP_ADC; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12); -} - -/** - * @brief Encode a ap_adc struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param ap_adc C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) -{ - return mavlink_msg_ap_adc_pack(system_id, component_id, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); -} - -/** - * @brief Send a ap_adc message - * @param chan MAVLink channel to send the message - * - * @param adc1 ADC output 1 - * @param adc2 ADC output 2 - * @param adc3 ADC output 3 - * @param adc4 ADC output 4 - * @param adc5 ADC output 5 - * @param adc6 ADC output 6 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_uint16_t(buf, 0, adc1); - _mav_put_uint16_t(buf, 2, adc2); - _mav_put_uint16_t(buf, 4, adc3); - _mav_put_uint16_t(buf, 6, adc4); - _mav_put_uint16_t(buf, 8, adc5); - _mav_put_uint16_t(buf, 10, adc6); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, 12); -#else - mavlink_ap_adc_t packet; - packet.adc1 = adc1; - packet.adc2 = adc2; - packet.adc3 = adc3; - packet.adc4 = adc4; - packet.adc5 = adc5; - packet.adc6 = adc6; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)&packet, 12); -#endif -} - -#endif - -// MESSAGE AP_ADC UNPACKING - - -/** - * @brief Get field adc1 from ap_adc message - * - * @return ADC output 1 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field adc2 from ap_adc message - * - * @return ADC output 2 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field adc3 from ap_adc message - * - * @return ADC output 3 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field adc4 from ap_adc message - * - * @return ADC output 4 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 6); -} - -/** - * @brief Get field adc5 from ap_adc message - * - * @return ADC output 5 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 8); -} - -/** - * @brief Get field adc6 from ap_adc message - * - * @return ADC output 6 - */ -static inline uint16_t mavlink_msg_ap_adc_get_adc6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 10); -} - -/** - * @brief Decode a ap_adc message into a struct - * - * @param msg The message to decode - * @param ap_adc C-struct to decode the message contents into - */ -static inline void mavlink_msg_ap_adc_decode(const mavlink_message_t* msg, mavlink_ap_adc_t* ap_adc) -{ -#if MAVLINK_NEED_BYTE_SWAP - ap_adc->adc1 = mavlink_msg_ap_adc_get_adc1(msg); - ap_adc->adc2 = mavlink_msg_ap_adc_get_adc2(msg); - ap_adc->adc3 = mavlink_msg_ap_adc_get_adc3(msg); - ap_adc->adc4 = mavlink_msg_ap_adc_get_adc4(msg); - ap_adc->adc5 = mavlink_msg_ap_adc_get_adc5(msg); - ap_adc->adc6 = mavlink_msg_ap_adc_get_adc6(msg); -#else - memcpy(ap_adc, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index 6a2e90cae5..cd59317f8f 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -185,65 +185,11 @@ static void mavlink_test_meminfo(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } -static void mavlink_test_ap_adc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_ap_adc_t packet_in = { - 17235, - 17339, - 17443, - 17547, - 17651, - 17755, - }; - mavlink_ap_adc_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.adc1 = packet_in.adc1; - packet1.adc2 = packet_in.adc2; - packet1.adc3 = packet_in.adc3; - packet1.adc4 = packet_in.adc4; - packet1.adc5 = packet_in.adc5; - packet1.adc6 = packet_in.adc6; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_pack(system_id, component_id, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_ap_adc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.adc1 , packet1.adc2 , packet1.adc3 , packet1.adc4 , packet1.adc5 , packet1.adc6 ); - mavlink_msg_ap_adc_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; ifree memory - - raw ADC output - ADC output 1 - ADC output 2 - ADC output 3 - ADC output 4 - ADC output 5 - ADC output 6 - - diff --git a/libraries/GCS_MAVLink/message_definitions/test.xml b/libraries/GCS_MAVLink/message_definitions/test.xml deleted file mode 100644 index 43a11e3d13..0000000000 --- a/libraries/GCS_MAVLink/message_definitions/test.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - 3 - - - Test all field types - char - string - uint8_t - uint16_t - uint32_t - uint64_t - int8_t - int16_t - int32_t - int64_t - float - double - uint8_t_array - uint16_t_array - uint32_t_array - uint64_t_array - int8_t_array - int16_t_array - int32_t_array - int64_t_array - float_array - double_array - - - From 7e0bff9cbe026f58a71c8ee98fddc2cb2be14581 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:35:23 +0200 Subject: [PATCH 11/31] Egg_drop has a 0..100 range --- ArduPlane/WP_activity.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/WP_activity.pde b/ArduPlane/WP_activity.pde index d497409342..474644cb90 100644 --- a/ArduPlane/WP_activity.pde +++ b/ArduPlane/WP_activity.pde @@ -32,10 +32,10 @@ void egg_waypoint() if(g.waypoint_index == 3){ if(wp_distance < egg_dist){ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (-45*10.31); + g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 100; } }else{ - g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 1500 + (45*10.31); + g_rc_function[RC_Channel_aux::k_egg_drop]->servo_out = 0; } } } From d3280bdd16a2b6ac04ce4b2b8b6ee9278acd9ab1 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:36:47 +0200 Subject: [PATCH 12/31] Added comments explaining when should the function be used --- ArduPlane/radio.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 11513fba41..f989d147e7 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -248,6 +248,8 @@ static void trim_radio() } // update the g_rc_function array from pointers to rc_x channels +// This should be done periodically because the user might change the configuration and +// expects the changes to take effect instantly static void update_aux_servo_function() { RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos From 386e80eaf600a4225adb58d6650b228bb60c335b Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:57:27 +0200 Subject: [PATCH 13/31] Added comments and TODO --- libraries/RC_Channel/RC_Channel.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 1519611c4b..2cc4df73d3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -106,6 +106,7 @@ class RC_Channel{ /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function +/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements class RC_Channel_aux : public RC_Channel{ public: /// Constructor @@ -137,6 +138,8 @@ public: k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; + // TODO It would be great if the "packed" attribute could be added to this somehow + // It would probably save some memory. But it can only be added to enums and not to typedefs :( AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func AP_Aux_srv_func function; // 0=disabled, 1=mount yaw (pan), 2=mount pitch (tilt), 3=mount roll, 4=camera trigger, 5=camera open, 6=flap, 7=flap auto, 8=aileron, 9=flaperon, 10=egg drop, 11=manual From b36d0352f376b3e7a1ceb349782eaa9e5008b22b Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sat, 10 Sep 2011 23:58:25 +0200 Subject: [PATCH 14/31] 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 d84f80cb0d44b94621899c53bea706d043cbf62c Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:34:47 +0200 Subject: [PATCH 15/31] Moved enum values around to make code simpler to read and use --- libraries/RC_Channel/RC_Channel.cpp | 14 +++++++------- libraries/RC_Channel/RC_Channel.h | 26 +++++++++++++------------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 2673f26e53..c1be664e2b 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -264,21 +264,21 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) case k_none: // disabled return; break; + case k_manual: // manual + radio_out = radio_in; + break; + case k_flap: // flaps + case k_flap_auto: // flaps automated + case k_aileron: // aileron + case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) case k_mount_yaw: // mount yaw (pan) case k_mount_pitch: // mount pitch (tilt) case k_mount_roll: // mount roll case k_cam_trigger: // camera trigger case k_cam_open: // camera open - case k_flap: // flaps - case k_flap_auto: // flaps automated - case k_aileron: // aileron - case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing) case k_egg_drop: // egg drop case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning break; - case k_manual: // manual - radio_out = radio_in; - break; } APM_RC.OutputCh(ch_nr, radio_out); diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 2cc4df73d3..6f8b3c6d17 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -123,18 +123,18 @@ public: typedef enum { - k_none = 0, // 0=disabled - k_mount_yaw = 1, // 1=mount yaw (pan) - k_mount_pitch = 2, // 2=mount pitch (tilt) - k_mount_roll = 3, // 3=mount roll - k_cam_trigger = 4, // 4=camera trigger - k_cam_open = 5, // 5=camera open - k_flap = 6, // 6=flap - k_flap_auto = 7, // 7=flap automated - k_aileron = 8, // 8=aileron - k_flaperon = 9, // 9=flaperon (flaps and aileron combined, needs two independent servos one for each wing) - k_egg_drop = 10, // 10=egg drop - k_manual = 11, // 11=manual, just pass-thru the RC in signal + k_none = 0, // disabled + k_manual = 1, // manual, just pass-thru the RC in signal + k_flap = 2, // flap + k_flap_auto = 3, // flap automated + k_aileron = 4, // aileron + k_flaperon = 5, // flaperon (flaps and aileron combined, needs two independent servos one for each wing) + k_mount_yaw = 6, // mount yaw (pan) + k_mount_pitch = 7, // mount pitch (tilt) + k_mount_roll = 8, // mount roll + k_cam_trigger = 9, // camera trigger + k_cam_open = 10, // camera open + k_egg_drop = 11, // egg drop k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; @@ -142,7 +142,7 @@ public: // It would probably save some memory. But it can only be added to enums and not to typedefs :( AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func - AP_Aux_srv_func function; // 0=disabled, 1=mount yaw (pan), 2=mount pitch (tilt), 3=mount roll, 4=camera trigger, 5=camera open, 6=flap, 7=flap auto, 8=aileron, 9=flaperon, 10=egg drop, 11=manual + AP_Aux_srv_func function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units From 84f108d598fe29868deb9290c68ca1156745f40e Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:36:39 +0200 Subject: [PATCH 16/31] Fix manual values mapping --- libraries/AP_Mount/AP_Mount.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 5629e0ee3e..6506e5f046 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -124,31 +124,27 @@ void AP_Mount::update_mount() yaw_angle=100*_roam_angles.z; break; } - case k_none: - { - //do nothing - break; - } case k_manual: // radio manual control if (g_rc_function[RC_Channel_aux::k_mount_roll]) roll_angle = map(g_rc_function[RC_Channel_aux::k_mount_roll]->radio_in, g_rc_function[RC_Channel_aux::k_mount_roll]->radio_min, g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max, g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_roll]->radio_max); + g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max); if (g_rc_function[RC_Channel_aux::k_mount_pitch]) pitch_angle = map(g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_in, g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_min, g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max, g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_pitch]->radio_max); + g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max); if (g_rc_function[RC_Channel_aux::k_mount_yaw]) yaw_angle = map(g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_in, g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_min, g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max, g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min, - g_rc_function[RC_Channel_aux::k_mount_yaw]->radio_max); + g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max); break; + case k_none: default: { //do nothing From 8aeee578b4524f50ef4b7c67a9e642dfd677fe34 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:40:13 +0200 Subject: [PATCH 17/31] For now point the camera manually via the RC inputs --- ArduPlane/ArduPlane.pde | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 45b95b69ee..e99afc1db1 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -577,7 +577,13 @@ static void medium_loop() { #if CAMERA == ENABLED // TODO replace home with a POI coming from a MavLink message or command - camera_mount.set_GPS_target(home); + //camera_mount.set_GPS_target(home); + + // For now point the camera manually via the RC inputs (later remove these two lines) + // for simple dcm tests, replace k_manual with k_stabilise + camera_mount.set_mode(AP_Mount::k_manual); + camera_mount.update_mount(); + g.camera.trigger_pic_cleanup(); #endif From dd19a7302dfcb135cf22d4f984f932bd60bc5d6d Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 02:41:48 +0200 Subject: [PATCH 18/31] 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 51a8ec0ba8af9f5823c5faf4eae9c50531fd555e Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 04:04:02 +0200 Subject: [PATCH 19/31] 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 20/31] 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 21/31] Fix RC range reversal --- ArduPlane/ArduPlane.pde | 4 ++-- ArduPlane/Parameters.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index e99afc1db1..f6b39a036c 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPilotMega V2.23" +#define THISFIRMWARE "ArduPilotMega V2.24" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi @@ -581,7 +581,7 @@ static void medium_loop() // For now point the camera manually via the RC inputs (later remove these two lines) // for simple dcm tests, replace k_manual with k_stabilise - camera_mount.set_mode(AP_Mount::k_manual); + camera_mount.set_mode(AP_Mount::k_stabilise); camera_mount.update_mount(); g.camera.trigger_pic_cleanup(); diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f0d930b1fb..7b62d9a8fa 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 11; + static const uint16_t k_format_version = 12; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) From 9f14c5f578e681038b02848514bee4cc9d9eeae4 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 04:12:46 +0200 Subject: [PATCH 22/31] Fix RC range reversal --- libraries/RC_Channel/RC_Channel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index c1be664e2b..7c497c828e 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -121,7 +121,7 @@ RC_Channel::calc_pwm(void) { if(_type == RC_CHANNEL_RANGE){ pwm_out = range_to_pwm(); - radio_out = pwm_out + radio_min; + radio_out = (_reverse >=0 ? pwm_out + radio_min : radio_max - pwm_out); }else if(_type == RC_CHANNEL_ANGLE_RAW){ pwm_out = (float)servo_out * .1; From 93cbe664d691a09ae74253dd5ca11f82f1db0c49 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 17:41:18 +0200 Subject: [PATCH 23/31] 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 c5fd79202436fff009470aaaebf00745f74ce6fb Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 18:32:24 +0200 Subject: [PATCH 24/31] 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 213969202a5a5086959a7d957a4f4c554bf4d657 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 19:13:01 +0200 Subject: [PATCH 25/31] 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 26/31] Moved update_aux_servo_function() to the RC_Channel_aux.* files. This simplifies code sharing between ArduCopter and Arduplane at the expense of 48bytes. Moved CH_x defines out of the defines.h file and into the library where they belong --- ArduPlane/ArduPlane.pde | 2 +- ArduPlane/defines.h | 13 ---------- ArduPlane/radio.pde | 33 +++---------------------- libraries/APM_RC/APM_RC.h | 11 +++++++++ libraries/RC_Channel/RC_Channel_aux.cpp | 27 ++++++++++++++++++++ libraries/RC_Channel/RC_Channel_aux.h | 4 ++- 6 files changed, 46 insertions(+), 44 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 02884aab63..d802efe651 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -735,7 +735,7 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); - update_aux_servo_function(); + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); #if CAMERA == ENABLED camera_mount.update_mount_type(); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index fff75fda4f..5c5b01b10f 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -41,19 +41,6 @@ #define GPS_PROTOCOL_MTK16 6 #define GPS_PROTOCOL_AUTO 7 -// Radio channels -// Note channels are from 0! -// -// XXX these should be CH_n defines from RC.h at some point. -#define CH_1 0 -#define CH_2 1 -#define CH_3 2 -#define CH_4 3 -#define CH_5 4 -#define CH_6 5 -#define CH_7 6 -#define CH_8 7 - #define CH_ROLL CH_1 #define CH_PITCH CH_2 #define CH_THROTTLE CH_3 diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 4f7292879d..6311f63e3e 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -195,7 +195,7 @@ static void trim_control_surfaces() g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel }else{ elevon1_trim = ch1_temp; @@ -212,7 +212,7 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } static void trim_radio() @@ -228,7 +228,7 @@ static void trim_radio() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -244,30 +244,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron] != NULL) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); -} - -// update the g_rc_function array from pointers to rc_x channels -// This should be done periodically because the user might change the configuration and -// expects the changes to take effect instantly -static void update_aux_servo_function() -{ - // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function - RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos - aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)g.rc_5.function.get(); - aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)g.rc_6.function.get(); - aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)g.rc_7.function.get(); - aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)g.rc_8.function.get(); - - // Assume that no auxiliary function is used - for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) - { - g_rc_function[i] = NULL; - } - - // assign the RC channel to each function - g_rc_function[aux_servo_function[CH_5]] = &g.rc_5; - g_rc_function[aux_servo_function[CH_6]] = &g.rc_6; - g_rc_function[aux_servo_function[CH_7]] = &g.rc_7; - g_rc_function[aux_servo_function[CH_8]] = &g.rc_8; + if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); } diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 236f85593d..bcac692abe 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -5,6 +5,17 @@ #define MIN_PULSEWIDTH 900 #define MAX_PULSEWIDTH 2100 +// Radio channels +// Note channels are from 0! +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + #include class APM_RC_Class diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index f6ceb5fed5..caa7affb61 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -3,6 +3,8 @@ #include #include "RC_Channel_aux.h" +extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function + int16_t RC_Channel_aux::closest_limit(int16_t angle) { @@ -67,3 +69,28 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) APM_RC.OutputCh(ch_nr, radio_out); } + +// update the g_rc_function array from pointers to rc_x channels +// This should be done periodically because the user might change the configuration and +// expects the changes to take effect instantly +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8) +{ + // positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function + RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos + aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)rc_5->function.get(); + aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)rc_6->function.get(); + aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)rc_7->function.get(); + aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)rc_8->function.get(); + + // Assume that no auxiliary function is used + for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++) + { + g_rc_function[i] = NULL; + } + + // assign the RC channel to each function + g_rc_function[aux_servo_function[CH_5]] = rc_5; + g_rc_function[aux_servo_function[CH_6]] = rc_6; + g_rc_function[aux_servo_function[CH_7]] = rc_7; + g_rc_function[aux_servo_function[CH_8]] = rc_8; +} diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 7e7fd4f167..8fae791fdc 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /// @file RC_Channel_aux.h -/// @brief RC_Channel manager, with EEPROM-backed storage of constants. +/// @brief RC_Channel manager for Channels 5..8, with EEPROM-backed storage of constants. /// @author Amilcar Lucas #ifndef RC_CHANNEL_AUX_H_ @@ -53,4 +53,6 @@ public: }; +void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8); + #endif /* RC_CHANNEL_AUX_H_ */ From 46f9d4cec85d2ae82d5ed30ebb1a4c08f71a243c Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Sun, 11 Sep 2011 23:25:06 +0200 Subject: [PATCH 27/31] 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 77c798abd5480e6dc2843563b34ad819a403b087 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Mon, 12 Sep 2011 00:02:47 +0200 Subject: [PATCH 28/31] Use G_RC_AUX macro to simplify accessing the auxiliary servos --- ArduPlane/radio.pde | 48 ++++++++------------------- libraries/AP_Camera/AP_Camera.cpp | 5 +-- libraries/AP_DCM/AP_DCM.h | 2 +- libraries/AP_Mount/AP_Mount.cpp | 9 ++--- libraries/RC_Channel/RC_Channel.h | 1 + libraries/RC_Channel/RC_Channel_aux.h | 3 ++ 6 files changed, 23 insertions(+), 45 deletions(-) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 6311f63e3e..ed3d1782b2 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -23,46 +23,26 @@ static void init_rc_in() g.channel_throttle.dead_zone = 6; //set auxiliary ranges - if (g_rc_function[RC_Channel_aux::k_flap]) { - g_rc_function[RC_Channel_aux::k_flap]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_flap_auto]) { - g_rc_function[RC_Channel_aux::k_flap_auto]->set_range(0,100); - } - if (g_rc_function[RC_Channel_aux::k_aileron]) { - g_rc_function[RC_Channel_aux::k_aileron]->set_angle(SERVO_MAX); - } - if (g_rc_function[RC_Channel_aux::k_flaperon]) { - g_rc_function[RC_Channel_aux::k_flaperon]->set_range(0,100); - } + G_RC_AUX(k_flap)->set_range(0,100); + G_RC_AUX(k_flap_auto)->set_range(0,100); + G_RC_AUX(k_aileron)->set_angle(SERVO_MAX); + G_RC_AUX(k_flaperon)->set_range(0,100); #if CAMERA == ENABLED - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) { - g_rc_function[RC_Channel_aux::k_mount_yaw]->set_range( + G_RC_AUX(k_mount_yaw)->set_range( g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) { - g_rc_function[RC_Channel_aux::k_mount_pitch]->set_range( + G_RC_AUX(k_mount_pitch)->set_range( g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_mount_roll]) { - g_rc_function[RC_Channel_aux::k_mount_roll]->set_range( + G_RC_AUX(k_mount_roll)->set_range( g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) { - g_rc_function[RC_Channel_aux::k_cam_trigger]->set_range( + G_RC_AUX(k_cam_trigger)->set_range( g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); - } - if (g_rc_function[RC_Channel_aux::k_cam_open]) { - g_rc_function[RC_Channel_aux::k_cam_open]->set_range(0,100); - } + G_RC_AUX(k_cam_open)->set_range(0,100); #endif - if (g_rc_function[RC_Channel_aux::k_egg_drop]) { - g_rc_function[RC_Channel_aux::k_egg_drop]->set_range(0,100); - } + G_RC_AUX(k_egg_drop)->set_range(0,100); } static void init_rc_out() @@ -195,7 +175,7 @@ static void trim_control_surfaces() g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel }else{ elevon1_trim = ch1_temp; @@ -212,7 +192,7 @@ static void trim_control_surfaces() g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } static void trim_radio() @@ -228,7 +208,7 @@ static void trim_radio() g.channel_pitch.radio_trim = g.channel_pitch.radio_in; //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel + G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel } else { elevon1_trim = ch1_temp; @@ -244,5 +224,5 @@ static void trim_radio() g.channel_pitch.save_eeprom(); //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); - if (g_rc_function[RC_Channel_aux::k_aileron]) g_rc_function[RC_Channel_aux::k_aileron]->save_eeprom(); + G_RC_AUX(k_aileron)->save_eeprom(); } diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 6fed9b86cb..ddfbc2d8a3 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -98,10 +98,7 @@ AP_Camera::trigger_pic_cleanup() case 0: case 2: case 3: - if (g_rc_function[RC_Channel_aux::k_cam_trigger]) - { - g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; - } + G_RC_AUX(k_cam_trigger)->radio_out = g_rc_function[RC_Channel_aux::k_cam_trigger]->radio_min; break; case 1: // TODO for some strange reason the function call bellow gives a linker error diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index bd705da8f8..6537c507a0 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -1,7 +1,7 @@ #ifndef AP_DCM_h #define AP_DCM_h -// teporarily include all other classes here +// temporarily include all other classes here // since this naming is a bit off from the // convention and the AP_DCM should be the top // header file diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 6cf76c9351..268533ee3b 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -156,12 +156,9 @@ void AP_Mount::update_mount() // write the results to the servos // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - if (g_rc_function[RC_Channel_aux::k_mount_roll]) - g_rc_function[RC_Channel_aux::k_mount_roll]->closest_limit(roll_angle/10); - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) - g_rc_function[RC_Channel_aux::k_mount_pitch]->closest_limit(pitch_angle/10); - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) - g_rc_function[RC_Channel_aux::k_mount_yaw]->closest_limit(yaw_angle/10); + G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); + G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); + G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); } void AP_Mount::set_mode(MountMode mode) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 4ff5990bbf..132e9146f3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -102,6 +102,7 @@ class RC_Channel{ int16_t _low; }; +// This is ugly, but it fixes compilation on arduino #include "RC_Channel_aux.h" #endif diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 8fae791fdc..6d8c228472 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -9,6 +9,9 @@ #include "RC_Channel.h" +// Macro to simplify accessing the auxiliary servos +#define G_RC_AUX(_t) if (g_rc_function[RC_Channel_aux::_t]) g_rc_function[RC_Channel_aux::_t] + /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function /// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements From b9383537e2cd70bc89d3806fe0e1d4f3c6ec5e7f Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Mon, 12 Sep 2011 00:43:54 +0200 Subject: [PATCH 29/31] 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 251c5875fbda693367423fe7f818d3aedd037f86 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 21:50:32 -0600 Subject: [PATCH 30/31] Disable stick mixing if in failsafe If trim values differ from failsafe channel values then stick mixing will adversely affect performance while in failsafe --- ArduPlane/Attitude.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 07300a65b6..63eefb30f5 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -61,7 +61,7 @@ static void stabilize() // Mix Stick input to allow users to override control surfaces // ----------------------------------------------------------- - if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) { + if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) { // TODO: use RC_Channel control_mix function? @@ -92,7 +92,7 @@ static void stabilize() // stick mixing performed for rudder for all cases including FBW unless disabled for higher modes // important for steering on the ground during landing // ----------------------------------------------- - if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) { + if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) { ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim; ch4_inf = fabs(ch4_inf); ch4_inf = min(ch4_inf, 400.0); From 8c9757a8d19d4ca96748fa23af254bb85ff83c42 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 11 Sep 2011 21:59:46 -0600 Subject: [PATCH 31/31] 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); }