From c22462bfd3dbc18e57f75b59729555e51794cbb6 Mon Sep 17 00:00:00 2001
From: Jean-Louis Naudin <jlnaudin@gmail.com>
Date: Wed, 13 Jun 2012 08:29:32 +0200
Subject: [PATCH] APMrover v2.20 - use RangeFinder optical SharpGP2Y instead of
ultrasonic sonar
Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
---
APMrover2/APM_Config.h | 6 ++--
APMrover2/APM_Config_Rover.h | 14 +++++----
APMrover2/APMrover2.pde | 55 +++++++++++++++++++++---------------
APMrover2/Attitude.pde | 31 --------------------
APMrover2/Parameters.h | 6 ++--
APMrover2/Parameters.pde | 2 +-
APMrover2/sensors.pde | 14 +++++----
APMrover2/system.pde | 36 ++++++++++++++++++++++-
APMrover2/test.pde | 38 +++++++++++++++++++------
9 files changed, 121 insertions(+), 81 deletions(-)
diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h
index 965e7df39..89c27e525 100644
--- a/APMrover2/APM_Config.h
+++ b/APMrover2/APM_Config.h
@@ -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
diff --git a/APMrover2/APM_Config_Rover.h b/APMrover2/APM_Config_Rover.h
index 8322d2026..6a62c6368 100644
--- a/APMrover2/APM_Config_Rover.h
+++ b/APMrover2/APM_Config_Rover.h
@@ -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
diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index d98c1373f..f9f25b5e3 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -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
diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde
index ba3a80e46..791dc171c 100644
--- a/APMrover2/Attitude.pde
+++ b/APMrover2/Attitude.pde
@@ -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();
}
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 1db9bb322..43e249dc0 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -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
diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
index 2a7e093b8..e7a0e75a2 100644
--- a/APMrover2/Parameters.pde
+++ b/APMrover2/Parameters.pde
@@ -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
diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde
index 08dc59c11..e1fc03e85 100644
--- a/APMrover2/sensors.pde
+++ b/APMrover2/sensors.pde
@@ -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;
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index e79f21357..1f67c1d6a 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -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;
diff --git a/APMrover2/test.pde b/APMrover2/test.pde
index 2c1c051d8..01f9b75a4 100644
--- a/APMrover2/test.pde
+++ b/APMrover2/test.pde
@@ -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
--
GitLab