diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h index 965e7df391cff6bd11bbeb236dcd87483dee5ea4..89c27e525fe14115756a07e23bc624199cd61fe8 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 8322d202643dcd3494fbe3b221d6abb1a5afa5f5..6a62c63683bd5a03fce82c2ad19a0fbff89483db 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 d98c1373f727ae4bc2f228ad3ae8046bbeb3aeac..f9f25b5e3a239827931755f8986a238b617e5d08 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 ba3a80e4601d87374ce95ce668586324a9545053..791dc171c0eaf0fecb85ec0d8a8ab41adfe6eaa3 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 1db9bb322e0f5fe01d379225784f3fa817aabd2e..43e249dc045823b63a6c515306e6da89b61e7e22 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 2a7e093b8c3f528575b57cc8bbe5faac6e19cfaa..e7a0e75a2bea469010cb8c101695c1e4850a1170 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 08dc59c11c9d3efa0a4ae60eba7df5b6a7b41f11..e1fc03e85991716f4632a9c69f317c3b1e3bd9cb 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 e79f213577cc1dd84aa25c131a63bdf365564aac..1f67c1d6a80c055c5fda40212bbb2c3625c1e6cb 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 2c1c051d85acb4949667edcb886c17fe87448b3d..01f9b75a4efcd15530d5ecf707968bb5023434f5 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