zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

APMrover v2.20 - use RangeFinder optical SharpGP2Y instead of ultrasonic sonar 

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
mission-4.1.18
Jean-Louis Naudin 13 years ago
parent
ff0659535e
commit
9355a159fc
9 changed files with 121 additions and 81 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      APMrover2/APM_Config.h
  2. 14
      APMrover2/APM_Config_Rover.h
  3. 55
      APMrover2/APMrover2.pde
  4. 31
      APMrover2/Attitude.pde
  5. 6
      APMrover2/Parameters.h
  6. 2
      APMrover2/Parameters.pde
  7. 14
      APMrover2/sensors.pde
  8. 36
      APMrover2/system.pde
  9. 38
      APMrover2/test.pde

6
APMrover2/APM_Config.h
Unescape View File

@ -8,9 +8,9 @@ @@ -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 @@ @@ -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

14
APMrover2/APM_Config_Rover.h
Unescape View File

@ -16,17 +16,19 @@ @@ -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)... @@ -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

55
APMrover2/APMrover2.pde
Unescape View File

@ -1,31 +1,26 @@ @@ -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; @@ -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() @@ -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() @@ -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() @@ -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) @@ -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) @@ -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

31
APMrover2/Attitude.pde
Unescape View File

@ -76,34 +76,6 @@ static void calc_nav_roll() @@ -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) @@ -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();
}

6
APMrover2/Parameters.h
Unescape View File

@ -83,7 +83,7 @@ public: @@ -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: @@ -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: @@ -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

2
APMrover2/Parameters.pde
Unescape View File

@ -105,7 +105,7 @@ static const AP_Param::Info var_info[] PROGMEM = { @@ -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

14
APMrover2/sensors.pde
Unescape View File

@ -1,21 +1,23 @@ @@ -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;

36
APMrover2/system.pde
Unescape View File

@ -195,11 +195,45 @@ static void init_ardupilot() @@ -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;

38
APMrover2/test.pde
Unescape View File

@ -18,7 +18,9 @@ static int8_t test_relay(uint8_t argc, const Menu::arg *argv); @@ -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 = { @@ -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) @@ -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) @@ -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

Write Preview
Loading…
Cancel
Save