mirror of https://github.com/ArduPilot/ardupilot
APMrover v2.20 - use RangeFinder optical SharpGP2Y instead of ultrasonic sonar
Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
This commit is contained in:
Jean-Louis Naudin
parent
ff0659535e
commit
9355a159fc
|
|
@ -8,9 +8,9 @@
|
|||
//#define SERIAL3_BAUD 38400
|
||||
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
|
||||
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
|
||||
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
|
||||
#define LITE DISABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
|
||||
#define LITE ENABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
|
||||
// if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2
|
||||
|
||||
#define CLI_ENABLED ENABLED
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
#define CLOSED_LOOP_NAV ENABLED
|
||||
#define AUTO_WP_RADIUS DISABLED
|
||||
|
||||
#define TRACE DISABLED
|
||||
#define TRACE ENABLED
|
||||
|
||||
//#include "APM_Config_HILmode.h" // for test in HIL mode with AeroSIM Rc 3.83
|
||||
#include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder
|
||||
|
|
|
|||
|
|
@ -16,17 +16,19 @@
|
|||
#define SONAR_TRIGGER 200 // trigger distance in cm
|
||||
|
||||
#if LITE == DISABLED
|
||||
#define MAGNETOMETER ENABLED
|
||||
#define LOGGING_ENABLED ENABLED
|
||||
|
||||
#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
|
||||
#define PARAM_DECLINATION 0.18 // Paris
|
||||
#endif
|
||||
|
||||
// for an accurate navigation a magnetometer must be used with the APM1
|
||||
#define MAGNETOMETER ENABLED
|
||||
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
|
||||
//#define PARAM_DECLINATION 0.18 // Paris
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Serial port speeds.
|
||||
//
|
||||
#define SERIAL0_BAUD 115200
|
||||
#define SERIAL3_BAUD 115200
|
||||
#define SERIAL3_BAUD 57600
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// GPS_PROTOCOL
|
||||
|
|
@ -59,7 +61,7 @@ the when the rover approach the wp, it slow down to 4 m/s (TRIM_ARSPD_CM)...
|
|||
This feature works only if the ROV_AWPR_NAV is set to 0
|
||||
*/
|
||||
|
||||
#define BOOSTER 2 // booster factor x2
|
||||
#define BOOSTER 1 // booster factor x1 = 1 or x2 = 2
|
||||
#define AUTO_WP_RADIUS DISABLED
|
||||
#define AIRSPEED_CRUISE 4 // 4m/s
|
||||
#define THROTTLE_SLEW_LIMIT 2 // set to 2 for a smooth acceleration by 0.2 step
|
||||
|
|
|
|||
|
|
@ -1,31 +1,26 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "APMrover v2.16a 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)
|
||||
#define THISFIRMWARE "APMrover v2.20 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
|
||||
|
||||
// This is the APMrover firmware derived from the Arduplane v2.32 by Jean-Louis Naudin (JLN)
|
||||
/*
|
||||
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin
|
||||
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
|
||||
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-20
|
||||
//
|
||||
// JLN updates: last update 2012-06-13
|
||||
// DOLIST:
|
||||
//
|
||||
//
|
||||
//-------------------------------------------------------------------------------------------------------------------------
|
||||
// Dev Startup : 2012-04-21
|
||||
//
|
||||
// 2012-06-13: use RangeFinder optical SharpGP2Y instead of ultrasonic sonar
|
||||
// 2012-05-13: added Test sonar
|
||||
// 2012-05-17: added speed_boost during straight line
|
||||
// 2012-05-17: New update about the throttle rate control based on the field test done by Franco Borasio (Thanks Franco..)
|
||||
// 2012-05-15: The Throttle rate can be controlled by the THROTTLE_SLEW_LIMIT (the value give the step increase, 1 = 0.1)
|
||||
|
|
@ -294,12 +289,16 @@ GCS_MAVLINK gcs3;
|
|||
//
|
||||
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);
|
||||
*/
|
||||
AP_AnalogSource_Arduino sonar_analog_source(A0); // use AN0 analog pin for APM2 on left
|
||||
AP_RangeFinder_SharpGP2Y sonar(&sonar_analog_source, &sonar_mode_filter);
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -817,14 +816,14 @@ static void fast_loop()
|
|||
|
||||
#if LITE == DISABLED
|
||||
ahrs.update();
|
||||
#endif
|
||||
// Read Sonar
|
||||
// ----------
|
||||
# if CONFIG_SONAR == ENABLED
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
if(g.sonar_enabled){
|
||||
sonar_dist = sonar.read();
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// uses the yaw from the DCM to give more accurate turns
|
||||
calc_bearing_error();
|
||||
|
|
@ -884,19 +883,23 @@ static void medium_loop()
|
|||
calc_gndspeed_undershoot();
|
||||
}
|
||||
|
||||
#if LITE == DISABLED
|
||||
//#if LITE == DISABLED
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if (g.compass_enabled && compass.read()) {
|
||||
ahrs.set_compass(&compass);
|
||||
// Calculate heading
|
||||
Matrix3f m = ahrs.get_dcm_matrix();
|
||||
#if LITE == DISABLED
|
||||
Matrix3f m = ahrs.get_dcm_matrix();
|
||||
compass.calculate(m);
|
||||
#else
|
||||
compass.calculate(0,0); // roll = 0, pitch = 0
|
||||
#endif
|
||||
compass.null_offsets();
|
||||
} else {
|
||||
ahrs.set_compass(NULL);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
//#endif
|
||||
/*{
|
||||
Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
|
||||
Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
|
||||
|
|
@ -1027,8 +1030,11 @@ static void slow_loop()
|
|||
#endif
|
||||
|
||||
#if TRACE == ENABLED
|
||||
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"),
|
||||
(float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
|
||||
// Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"),
|
||||
// (float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
|
||||
// Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"),
|
||||
// g.command_total, g.command_index, nav_command_index);
|
||||
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d\n"), (float)ground_course/100, (int)sonar_dist);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
|
@ -1077,12 +1083,12 @@ static void update_GPS(void)
|
|||
} else if (ENABLE_AIR_START == 0) {
|
||||
init_home();
|
||||
}
|
||||
#if LITE == DISABLED
|
||||
//#if LITE == DISABLED
|
||||
if (g.compass_enabled) {
|
||||
// Set compass declination automatically
|
||||
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
|
||||
}
|
||||
#endif
|
||||
//#endif
|
||||
ground_start_count = 0;
|
||||
}
|
||||
}
|
||||
|
|
@ -1098,8 +1104,13 @@ static void update_GPS(void)
|
|||
ground_course = hdg * 100;
|
||||
ground_course = ahrs.yaw_sensor;
|
||||
} else {
|
||||
#endif
|
||||
ground_course = g_gps->ground_course;
|
||||
#endif
|
||||
long magheading;
|
||||
magheading = ToDeg(compass.heading) * 100;
|
||||
if (magheading > 36000) magheading -= 36000;
|
||||
if (magheading < 0) magheading += 36000;
|
||||
|
||||
ground_course = magheading;
|
||||
#if LITE == DISABLED
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -76,34 +76,6 @@ static void calc_nav_roll()
|
|||
|
||||
}
|
||||
|
||||
/*****************************************
|
||||
* Roll servo slew limit
|
||||
*****************************************/
|
||||
/*
|
||||
float roll_slew_limit(float servo)
|
||||
{
|
||||
static float last;
|
||||
float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f);
|
||||
last = servo;
|
||||
return temp;
|
||||
}*/
|
||||
|
||||
/*****************************************
|
||||
* Throttle slew limit
|
||||
*****************************************/
|
||||
static void throttle_slew_limit()
|
||||
{
|
||||
/*
|
||||
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
|
||||
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
|
||||
static void reset_I(void)
|
||||
|
|
@ -152,9 +124,6 @@ static void set_servos(void)
|
|||
if (control_mode >= FLY_BY_WIRE_B) {
|
||||
// convert 0 to 100% into PWM
|
||||
g.channel_throttle.calc_pwm();
|
||||
/* only do throttle slew limiting in modes where throttle
|
||||
control is automatic */
|
||||
throttle_slew_limit();
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@ public:
|
|||
k_param_pack_capacity,
|
||||
k_param_airspeed_offset,
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if LITE == DISABLED
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
k_param_sonar_enabled,
|
||||
k_param_sonar_type,
|
||||
#endif
|
||||
|
|
@ -334,7 +334,7 @@ public:
|
|||
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
||||
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if LITE == DISABLED
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
AP_Int8 sonar_enabled;
|
||||
AP_Int8 sonar_type; // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)
|
||||
#endif
|
||||
|
|
@ -471,7 +471,7 @@ public:
|
|||
pack_capacity (HIGH_DISCHARGE),
|
||||
inverted_flight_ch (0),
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if LITE == DISABLED
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
sonar_enabled (SONAR_ENABLED),
|
||||
sonar_type (AP_RANGEFINDER_MAXSONARXL),
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -105,7 +105,7 @@ static const AP_Param::Info var_info[] PROGMEM = {
|
|||
GSCALAR(pack_capacity, "BATT_CAPACITY"),
|
||||
GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH"),
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if LITE == DISABLED
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
// @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
|
||||
|
|
|
|||
|
|
@ -1,21 +1,23 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
#if LITE == DISABLED
|
||||
// Sensors are not available in HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
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
|
||||
|
||||
#if LITE == DISABLED
|
||||
// Sensors are not available in HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
void ReadSCP1000(void) {}
|
||||
|
||||
static void init_barometer(void)
|
||||
{
|
||||
int flashcount = 0;
|
||||
|
|
|
|||
|
|
@ -195,11 +195,45 @@ static void init_ardupilot()
|
|||
compass.null_offsets_enable();
|
||||
}
|
||||
}
|
||||
#else
|
||||
I2c.begin();
|
||||
I2c.timeOut(20);
|
||||
|
||||
// I2c.setSpeed(true);
|
||||
|
||||
if (!compass.init()) {
|
||||
Serial.println("compass initialisation failed!");
|
||||
while (1) ;
|
||||
}
|
||||
|
||||
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft.
|
||||
compass.set_offsets(0,0,0); // set offsets to account for surrounding interference
|
||||
compass.set_declination(ToRad(0.0)); // set local difference between magnetic north and true north
|
||||
|
||||
Serial.print("Compass auto-detected as: ");
|
||||
switch( compass.product_id ) {
|
||||
case AP_COMPASS_TYPE_HIL:
|
||||
Serial.println("HIL");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5843:
|
||||
Serial.println("HMC5843");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5883L:
|
||||
Serial.println("HMC5883L");
|
||||
break;
|
||||
default:
|
||||
Serial.println("unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
delay(3000);
|
||||
|
||||
#endif
|
||||
// initialise sonar
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
init_sonar();
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#endif
|
||||
// Do GPS init
|
||||
g_gps = &g_gps_driver;
|
||||
|
|
|
|||
|
|
@ -18,7 +18,9 @@ static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
|||
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_vario(uint8_t argc, const Menu::arg *argv);
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
|
||||
|
|
@ -59,7 +61,9 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
|||
{"imu", test_imu},
|
||||
{"airspeed", test_airspeed},
|
||||
{"airpressure", test_pressure},
|
||||
{"vario", test_vario},
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
{"sonartest", test_sonar},
|
||||
#endif
|
||||
{"compass", test_mag},
|
||||
#elif HIL_MODE == HIL_MODE_SENSORS
|
||||
{"adc", test_adc},
|
||||
|
|
@ -662,12 +666,6 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_vario(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_rawgps(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
@ -690,6 +688,30 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
static int8_t
|
||||
test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
init_sonar();
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
if(g.sonar_enabled){
|
||||
sonar_dist = sonar.read();
|
||||
}
|
||||
Serial.printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
|
||||
|
||||
if(Serial.available() > 0){
|
||||
break;
|
||||
}
|
||||
}
|
||||
return (0);
|
||||
}
|
||||
#endif // SONAR == ENABLED
|
||||
#endif // HIL_MODE == HIL_MODE_DISABLED
|
||||
|
||||
#endif // CLI_ENABLED
|
||||
|
|
|
|||
Loading…
Reference in New Issue