diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h index 965e7df391..89c27e525f 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 8322d20264..6a62c63683 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 d98c1373f7..f9f25b5e3a 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 ba3a80e460..791dc171c0 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 1db9bb322e..43e249dc04 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 2a7e093b8c..e7a0e75a2b 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 08dc59c11c..e1fc03e859 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 e79f213577..1f67c1d6a8 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 2c1c051d85..01f9b75a4e 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