mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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 <jlnaudin@gmail.com>
This commit is contained in:
Jean-Louis Naudin
parent
f4975cb7fe
commit
4a4402556b
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -12,6 +12,9 @@
|
||||
|
||||
#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
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
@ -476,6 +490,8 @@ 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;
|
||||
// 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
|
||||
@ -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;
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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),
|
||||
|
||||
// ************************************************************
|
||||
|
||||
@ -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"),
|
||||
// ************************************************************
|
||||
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -69,7 +69,7 @@ static void process_next_command()
|
||||
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);
|
||||
restart_nav();
|
||||
return;
|
||||
} else {
|
||||
handle_no_commands();
|
||||
@ -144,3 +144,5 @@ static void process_non_nav_command()
|
||||
non_nav_command_ID = NO_COMMAND;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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,10 +189,43 @@
|
||||
# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
|
||||
#endif
|
||||
|
||||
#ifndef SONAR_ENABLED
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// 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
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// HIL_MODE OPTIONAL
|
||||
|
||||
@ -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
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -77,6 +77,11 @@ 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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user