From 4a4402556bdce0593ab72eb6ce7013916f0f70bc Mon Sep 17 00:00:00 2001 From: Jean-Louis Naudin Date: Mon, 14 May 2012 17:47:08 +0200 Subject: [PATCH] APMrover v2.1.3 : Added option (hold roll to full right + SW7 ON/OFF) to init_home during the wp_list reset Add ROV_SONAR_TRIG (default = 200 cm) - tested by Franco Restart_nav() added and heading bug correction, tested OK in the field RTL then stop update - Tested OK in the field Added SONAR detection for obstacle avoidance Signed-off-by: Jean-Louis Naudin --- APMrover2/APM_Config.h | 2 +- APMrover2/APM_Config_HILmode.h | 6 +-- APMrover2/APM_Config_Rover.h | 11 +++-- APMrover2/APMrover2.pde | 75 +++++++++++++++++++++++++--------- APMrover2/GCS_Mavlink.pde | 2 +- APMrover2/Log.pde | 2 +- APMrover2/Parameters.h | 30 ++++++++++---- APMrover2/Parameters.pde | 15 +++++-- APMrover2/commands.pde | 21 +++++++--- APMrover2/commands_logic.pde | 1 + APMrover2/commands_process.pde | 8 ++-- APMrover2/config.h | 48 +++++++++++++++++++++- APMrover2/control_modes.pde | 2 + APMrover2/defines.h | 3 ++ APMrover2/navigation.pde | 7 +++- APMrover2/radio.pde | 4 +- APMrover2/sensors.pde | 11 +++++ APMrover2/system.pde | 8 +++- 18 files changed, 197 insertions(+), 59 deletions(-) diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h index be6ea6f9d0..965e7df391 100644 --- a/APMrover2/APM_Config.h +++ b/APMrover2/APM_Config.h @@ -16,7 +16,7 @@ #define CLI_ENABLED ENABLED #define CLI_SLIDER_ENABLED DISABLED #define CLOSED_LOOP_NAV ENABLED -#define AUTO_WP_RADIUS ENABLED +#define AUTO_WP_RADIUS DISABLED #define TRACE DISABLED diff --git a/APMrover2/APM_Config_HILmode.h b/APMrover2/APM_Config_HILmode.h index 90d924c5d5..ad1ef404ce 100644 --- a/APMrover2/APM_Config_HILmode.h +++ b/APMrover2/APM_Config_HILmode.h @@ -33,8 +33,6 @@ #define MAGNETOMETER DISABLED #define LOGGING_ENABLED DISABLED -#define CH7_OPTION CH7_CH7_SAVE_WP - #define TURN_GAIN 5 #define CH7_OPTION CH7_SAVE_WP @@ -422,8 +420,8 @@ // Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow // users to start using the APM without running the WaypointWriter first. // -#define WP_RADIUS_DEFAULT 3 // meters -#define LOITER_RADIUS_DEFAULT 10 // 60 +#define WP_RADIUS_DEFAULT 1 // meters +#define LOITER_RADIUS_DEFAULT 5 #define USE_CURRENT_ALT TRUE #define ALT_HOLD_HOME 0 diff --git a/APMrover2/APM_Config_Rover.h b/APMrover2/APM_Config_Rover.h index b05e974cd2..ff8a0da3ab 100644 --- a/APMrover2/APM_Config_Rover.h +++ b/APMrover2/APM_Config_Rover.h @@ -12,9 +12,12 @@ #define AIRSPEED_SENSOR DISABLED +#define SONAR_ENABLED DISABLED +#define SONAR_TRIGGER 200 // trigger distance in cm + #if LITE == DISABLED #define MAGNETOMETER ENABLED - #define LOGGING_ENABLED ENABLED + #define LOGGING_ENABLED ENABLED #define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD #define PARAM_DECLINATION 0.18 // Paris @@ -46,7 +49,7 @@ #define TURN_GAIN 5 #define CLOSED_LOOP_NAV ENABLED // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch) -#define AUTO_WP_RADIUS ENABLED +#define AUTO_WP_RADIUS DISABLED #define MAX_DIST 50 //300 // max distance (in m) for the HEADALT mode #define SARSEC_BRANCH 50 // Long branch of the SARSEC pattern @@ -418,8 +421,8 @@ // Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow // users to start using the APM without running the WaypointWriter first. // -#define WP_RADIUS_DEFAULT 5 // meters -#define LOITER_RADIUS_DEFAULT 10 // 60 +#define WP_RADIUS_DEFAULT 1 // meters +#define LOITER_RADIUS_DEFAULT 5 // 60 #define USE_CURRENT_ALT TRUE #define ALT_HOLD_HOME 0 diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index ae3bec3aee..770b9a3732 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "APMrover v2.0c JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer +#define THISFIRMWARE "APMrover v2.13 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR // This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN) @@ -9,6 +9,8 @@ Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier Please contribute your ideas! +APMrover alpha version tester: Franco Borasio, Daniel Chapelat... + This firmware is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public @@ -16,7 +18,7 @@ License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. // -// JLN updates: last update 2012-05-04 +// JLN updates: last update 2012-05-14 // // DOLIST: // @@ -24,6 +26,12 @@ version 2.1 of the License, or (at your option) any later version. //------------------------------------------------------------------------------------------------------------------------- // Dev Startup : 2012-04-21 // +// 2012-05-14: Added option (hold roll to full right + SW7 ON/OFF) to init_home during the wp_list reset +// 2012-05-13: Add ROV_SONAR_TRIG (default = 200 cm) +// 2012-05-13: Restart_nav() added and heading bug correction, tested OK in the field +// 2012-05-12: RTL then stop update - Tested in the field +// 2012-05-11: The rover now STOP after the RTL... (special update for Franco...) +// 2012-05-11: Added SONAR detection for obstacle avoidance (alpha version for SONAR testing) // 2012-05-04: Added #define LITE ENABLED for the APM1280 or APM2560 CPU IMUless version // 2012-05-03: Successful missions tests with a full APM2560 kit (GPS MT3329 + magnetometer HMC5883L) // 2012-05-03: removing stick mixing in auto mode @@ -272,10 +280,23 @@ GCS_MAVLINK gcs0; GCS_MAVLINK gcs3; //////////////////////////////////////////////////////////////////////////////// -// PITOT selection +// SONAR selection //////////////////////////////////////////////////////////////////////////////// // ModeFilterInt16_Size5 sonar_mode_filter(2); +#if CONFIG_SONAR == ENABLED + #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC + AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); + #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN + AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN); + #endif + AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter); +#endif + +//////////////////////////////////////////////////////////////////////////////// +// PITOT selection +//////////////////////////////////////////////////////////////////////////////// +// #if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC AP_AnalogSource_ADC pitot_analog_source( &adc, @@ -284,14 +305,6 @@ AP_AnalogSource_ADC pitot_analog_source( &adc, AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0); #endif -#if SONAR_TYPE == MAX_SONAR_XL - AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); -#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(&pitot_analog_source, &sonar_mode_filter); -#endif - // Barometer filter AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration @@ -444,6 +457,7 @@ static float nav_gain_scaler = 1; // Direction held during phases of takeoff and landing // deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use static long hold_course = -1; // deg * 100 dir of plane +static bool rtl_complete = false; // There may be two active commands in Auto mode. // This indicates the active navigation command by index number @@ -475,7 +489,9 @@ static long airspeed_energy_error; static int airspeed_nudge; // Similar to airspeed_nudge, but used when no airspeed sensor. // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel -static int throttle_nudge = 0; +static int throttle_nudge = 0; +// The distance as reported by Sonar in cm – Values are 20 to 700 generally. +static int16_t sonar_dist; //////////////////////////////////////////////////////////////////////////////// // Ground speed @@ -538,8 +554,7 @@ static int16_t ground_temperature; //////////////////////////////////////////////////////////////////////////////// // Raw absolute pressure measurement (filtered). ADC units static unsigned long abs_pressure; -// Altitude from the sonar sensor. Meters. Not yet implemented. -static int sonar_alt; + // The altitude as reported by Baro in cm – Values can be quite high static int32_t baro_alt; @@ -791,6 +806,13 @@ static void fast_loop() #if LITE == DISABLED ahrs.update(); + // Read Sonar + // ---------- + # if CONFIG_SONAR == ENABLED + if(g.sonar_enabled){ + sonar_dist = sonar.read(); + } + #endif #endif // uses the yaw from the DCM to give more accurate turns @@ -818,7 +840,7 @@ static void fast_loop() // apply desired roll, pitch and yaw to the plane // ---------------------------------------------- - if (control_mode > MANUAL) + if (control_mode > STABILIZE) stabilize(); // write out the servo PWM values @@ -1070,9 +1092,10 @@ static void update_GPS(void) if (g.compass_enabled) { hdg=(ahrs.yaw_sensor / 100) % 360; ground_course = hdg * 100; + ground_course = ahrs.yaw_sensor; } else { #endif - ground_course = abs(g_gps->ground_course); + ground_course = g_gps->ground_course; #if LITE == DISABLED } #endif @@ -1094,7 +1117,7 @@ static void update_current_flight_mode(void) calc_nav_roll(); calc_throttle(); break; - } + } }else{ switch(control_mode){ case RTL: @@ -1112,6 +1135,13 @@ static void update_current_flight_mode(void) case STABILIZE: nav_roll = 0; nav_pitch = 0; + #if X_PLANE == ENABLED + // servo_out is for Sim control only + // --------------------------------- + g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); + g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); + g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle(); + #endif // throttle is passthrough break; @@ -1132,7 +1162,7 @@ static void update_current_flight_mode(void) // --------------------------------- g.channel_roll.servo_out = g.channel_roll.pwm_to_angle(); g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle(); - g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle(); + g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 @@ -1152,10 +1182,15 @@ static void update_navigation() switch(control_mode){ case LOITER: - case RTL: + case RTL: // no loitering around the wp with the rover, goes direct to the wp position case GUIDED: - update_loiter(); + // update_loiter(); + calc_nav_roll(); calc_bearing_error(); + if(verify_RTL()) + { g.channel_throttle.servo_out = g.throttle_min.get(); + set_mode(MANUAL); + } break; } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index a30056d6f6..b3b6d93b31 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -452,7 +452,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) chan, (float)airspeed / 100.0, (float)g_gps->ground_speed / 100.0, - (ahrs.yaw_sensor / 100) % 360, + ground_course, (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100 current_loc.alt / 100.0, 0); diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index c563c599f9..6500c4a10e 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -351,7 +351,7 @@ static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_ DataFlash.WriteByte(log_NumSats); DataFlash.WriteLong(log_Lattitude); DataFlash.WriteLong(log_Longitude); - DataFlash.WriteInt(sonar_alt); // This one is just temporary for testing out sonar in fixed wing + DataFlash.WriteInt(sonar_dist); // This one is just temporary for testing out sonar in fixed wing DataFlash.WriteLong(log_mix_alt); DataFlash.WriteLong(log_gps_alt); DataFlash.WriteLong(log_Ground_Speed); diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 8060df868a..951490164d 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -82,8 +82,12 @@ public: k_param_input_voltage, k_param_pack_capacity, k_param_airspeed_offset, - -// k_param_sonar_enabled, +#if HIL_MODE != HIL_MODE_ATTITUDE +#if LITE == DISABLED + k_param_sonar_enabled, + k_param_sonar_type, +#endif +#endif k_param_airspeed_enabled, // @@ -127,7 +131,7 @@ public: k_param_closed_loop_nav, k_param_auto_wp_radius, - k_param_nudgeoffset, + k_param_sonar_trigger, k_param_turn_gain, // ************************************************************ @@ -328,8 +332,12 @@ public: AP_Float input_voltage; AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger - - // AP_Int8 sonar_enabled; +#if HIL_MODE != HIL_MODE_ATTITUDE +#if LITE == DISABLED + AP_Int8 sonar_enabled; + AP_Int8 sonar_type; // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range) +#endif +#endif AP_Int8 airspeed_enabled; AP_Int8 flap_1_percent; AP_Int8 flap_1_speed; @@ -341,7 +349,7 @@ public: AP_Int8 closed_loop_nav; AP_Int8 auto_wp_radius; - AP_Int16 nudgeoffset; + AP_Int16 sonar_trigger; AP_Int16 turn_gain; // ************************************************************ @@ -460,8 +468,12 @@ public: input_voltage (INPUT_VOLTAGE), pack_capacity (HIGH_DISCHARGE), inverted_flight_ch (0), - -// sonar_enabled (SONAR_ENABLED), +#if HIL_MODE != HIL_MODE_ATTITUDE +#if LITE == DISABLED + sonar_enabled (SONAR_ENABLED), + sonar_type (AP_RANGEFINDER_MAXSONARXL), +#endif +#endif airspeed_enabled (AIRSPEED_SENSOR), // ************ APMrover parameters ************************ @@ -469,7 +481,7 @@ public: closed_loop_nav (CLOSED_LOOP_NAV), auto_wp_radius (AUTO_WP_RADIUS), - nudgeoffset (NUDGE_OFFSET), + sonar_trigger (SONAR_TRIGGER), turn_gain (TURN_GAIN), // ************************************************************ diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index c52d3832a9..e5f368c4f1 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -104,8 +104,17 @@ static const AP_Param::Info var_info[] PROGMEM = { GSCALAR(input_voltage, "INPUT_VOLTS"), GSCALAR(pack_capacity, "BATT_CAPACITY"), GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH"), - - //GSCALAR(sonar_enabled, "SONAR_ENABLE"), +#if HIL_MODE != HIL_MODE_ATTITUDE +#if LITE == DISABLED + // @Param: SONAR_ENABLE + // @DisplayName: Enable Sonar + // @Description: Setting this to Enabled(1) will enable the sonar. Setting this to Disabled(0) will disable the sonar + // @Values: 0:Disabled,1:Enabled + // @User: Standard + GSCALAR(sonar_enabled, "SONAR_ENABLE"), + GSCALAR(sonar_type, "SONAR_TYPE"), +#endif +#endif GSCALAR(airspeed_enabled, "ARSPD_ENABLE"), // ************************************************************ @@ -113,7 +122,7 @@ static const AP_Param::Info var_info[] PROGMEM = { GSCALAR(closed_loop_nav, "ROV_CL_NAV"), GSCALAR(auto_wp_radius, "ROV_AWPR_NAV"), - GSCALAR(nudgeoffset, "ROV_NUDGE"), + GSCALAR(sonar_trigger, "ROV_SONAR_TRIG"), GSCALAR(turn_gain, "ROV_GAIN"), // ************************************************************ diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index 0d853e1e71..eacb0f5b9a 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -12,6 +12,7 @@ void set_next_WP(struct Location *wp) void set_guided_WP(void) void init_home() + void restart_nav() ************************************************************ */ @@ -250,14 +251,14 @@ void init_home() home.alt = g_gps->altitude;; // Home is always 0 #else - struct Location temp = get_cmd_with_index(0); // JLN update - for HIL test only get the home param stored in the FPL - if (temp.alt > 0) { - home.lng = temp.lng; // Lon * 10**7 - home.lat = temp.lat; // Lat * 10**7 - } else { +// struct Location temp = get_cmd_with_index(0); // JLN update - for HIL test only get the home param stored in the FPL +// if (temp.alt > 0) { +// home.lng = temp.lng; // Lon * 10**7 +// home.lat = temp.lat; // Lat * 10**7 +// } else { home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 - } +// } gps_base_alt = g_gps->altitude;; // get the stored home altitude as the base ref for AGL calculation. home.alt = g_gps->altitude;; @@ -282,3 +283,11 @@ void init_home() } +static void restart_nav() +{ + reset_I(); + prev_WP = current_loc; + nav_command_ID = NO_COMMAND; + nav_command_index = 0; + process_next_command(); +} diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index c4241143e2..71b9ee4ef8 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -331,6 +331,7 @@ static bool verify_RTL() { if (wp_distance <= g.waypoint_radius) { gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); + rtl_complete = true; return true; }else{ return false; diff --git a/APMrover2/commands_process.pde b/APMrover2/commands_process.pde index b6e98f3076..c1ab1fb3c0 100644 --- a/APMrover2/commands_process.pde +++ b/APMrover2/commands_process.pde @@ -68,9 +68,9 @@ static void process_next_command() // we are out of commands! gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); - if (g.closed_loop_nav) { // JLN update - replay the FPL (CLOSED_LOOP_NAV) - change_command(1); - return; + if (g.closed_loop_nav) { // JLN update - replay the FPL (CLOSED_LOOP_NAV) + restart_nav(); + return; } else { handle_no_commands(); } @@ -144,3 +144,5 @@ static void process_non_nav_command() non_nav_command_ID = NO_COMMAND; } } + + diff --git a/APMrover2/config.h b/APMrover2/config.h index 9277dbb9fa..21950c9441 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -50,9 +50,17 @@ # define CONFIG_APM_HARDWARE APM_HARDWARE_APM1 #endif +#ifndef X_PLANE +# define X_PLANE DISABLED +#endif + +#if X_PLANE == ENABLED +# define LITE ENABLED +#else #ifndef LITE # define LITE DISABLED #endif +#endif #if defined( __AVR_ATmega1280__ ) #define CLI_ENABLED DISABLED @@ -181,8 +189,41 @@ # define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL, #endif -#ifndef SONAR_ENABLED -#define SONAR_ENABLED DISABLED +////////////////////////////////////////////////////////////////////////////// +// Sonar +// + +#ifndef CONFIG_SONAR_SOURCE +# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC +#endif + +#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC && CONFIG_ADC == DISABLED +# warning Cannot use ADC for CONFIG_SONAR_SOURCE, becaude CONFIG_ADC is DISABLED +# warning Defaulting CONFIG_SONAR_SOURCE to ANALOG_PIN +# undef CONFIG_SONAR_SOURCE +# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN +#endif + +#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC +# ifndef CONFIG_SONAR_SOURCE_ADC_CHANNEL +# define CONFIG_SONAR_SOURCE_ADC_CHANNEL 7 +# endif +#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN +# ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN +# define CONFIG_SONAR_SOURCE_ANALOG_PIN A0 +# endif +#else +# warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar +# undef SONAR_ENABLED +# define SONAR_ENABLED DISABLED +#endif + +#ifndef CONFIG_SONAR +# define CONFIG_SONAR ENABLED +#endif + +#ifndef SONAR_TRIGGER +# define SONAR_TRIGGER 60 // trigger distance in cm #endif ////////////////////////////////////////////////////////////////////////////// @@ -195,6 +236,9 @@ #if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode # undef GPS_PROTOCOL # define GPS_PROTOCOL GPS_PROTOCOL_NONE + + #undef CONFIG_SONAR + #define CONFIG_SONAR DISABLED #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde index 378a24ab0b..99cc5547bd 100644 --- a/APMrover2/control_modes.pde +++ b/APMrover2/control_modes.pde @@ -70,6 +70,8 @@ if (g.ch7_option == CH7_SAVE_WP){ // set to 1 g.command_total = 0; g.command_index =0; nav_command_index = 0; + if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home + ground_start_count = 5; #if X_PLANE == ENABLED Serial.printf_P(PSTR("*** RESET the FPL\n")); #endif diff --git a/APMrover2/defines.h b/APMrover2/defines.h index fe0f4e93da..c26495a768 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -30,6 +30,9 @@ #define SONAR 0 #define BARO 1 +#define SONAR_SOURCE_ADC 1 +#define SONAR_SOURCE_ANALOG_PIN 2 + // CH 7 control #define CH7_DO_NOTHING 0 #define CH7_SAVE_WP 1 diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index bae3fe7188..453049852b 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -76,7 +76,12 @@ static void calc_gndspeed_undershoot() } static void calc_bearing_error() -{ +{ + #if CONFIG_SONAR == ENABLED + if((g.sonar_enabled) && (sonar_dist < g.sonar_trigger)) { + nav_bearing += 9000; // if obstacle in front turn 90° right + } + #endif bearing_error = nav_bearing - ground_course; bearing_error = wrap_180(bearing_error); } diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index 5442ebe7db..9578c7f327 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -103,12 +103,12 @@ static void read_radio() if (g.channel_throttle.servo_out > 50) { if(g.airspeed_enabled == true) { airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); - airspeed_nudge = g.nudgeoffset + airspeed_nudge; + airspeed_nudge = NUDGE_OFFSET + airspeed_nudge; } else { throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); } } else { - airspeed_nudge = g.nudgeoffset; + airspeed_nudge = NUDGE_OFFSET; throttle_nudge = 0; } diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 1eeafe2e3c..08dc59c11c 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -5,6 +5,17 @@ void ReadSCP1000(void) {} +#if CONFIG_SONAR == ENABLED +static void init_sonar(void) +{ + #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC + sonar.calculate_scaler(g.sonar_type, 3.3); + #else + sonar.calculate_scaler(g.sonar_type, 5.0); + #endif +} +#endif + static void init_barometer(void) { int flashcount = 0; diff --git a/APMrover2/system.pde b/APMrover2/system.pde index beb85c23ef..dacb90c220 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -195,6 +195,10 @@ static void init_ardupilot() compass.null_offsets_enable(); } } + // initialise sonar + #if CONFIG_SONAR == ENABLED + init_sonar(); + #endif #endif #endif // Do GPS init @@ -358,8 +362,8 @@ static void set_mode(byte mode) break; case AUTO: - change_command(1); // restart to the 1st stored wp - //update_auto(); + rtl_complete = false; + restart_nav(); break; case RTL: