From b11b83e2cfcd68e9d48cf40d589bee747d1f8468 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Dec 2011 08:48:59 +1100 Subject: [PATCH 01/14] GPS: added FAKE_GPS_LOCK_TIME this allows you to fake up a GPS lock after a specific number of seconds --- libraries/AP_GPS/AP_GPS_MTK16.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index 96cc189b9c..659f60f71b 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp @@ -13,6 +13,7 @@ #include "AP_GPS_MTK16.h" #include +#include // Constructors //////////////////////////////////////////////////////////////// AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s) @@ -141,6 +142,15 @@ restart: parsed = true; +#ifdef FAKE_GPS_LOCK_TIME + if (millis() > FAKE_GPS_LOCK_TIME*1000) { + fix = true; + latitude = -35000000UL; + longitude = 149000000UL; + altitude = 584; + } +#endif + /* Waiting on clarification of MAVLink protocol! if(!_offset_calculated && parsed) { long tempd1 = date; From c1569e49a869a11f7c15629f292fbcade3a8689b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Dec 2011 09:06:08 +1100 Subject: [PATCH 02/14] fixed the "reboot on GPS lock" bug the problem was that with APM2 we may not have a 2nd GCS instance. The "gps alt: xxx" message was being unconditionally sent to both GCS instances, which caused a crash --- ArduCopter/GCS_Mavlink.pde | 4 +++- ArduPlane/GCS_Mavlink.pde | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 14e8cf1ce3..1eef88ec3f 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1799,5 +1799,7 @@ static void gcs_send_text_fmt(const prog_char_t *fmt, ...) vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap); va_end(ap); mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); - mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + if (gcs3.initialised) { + mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + } } diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index eee368cbf2..b2f5884cba 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -2103,5 +2103,7 @@ static void gcs_send_text_fmt(const prog_char_t *fmt, ...) vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap); va_end(ap); mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); - mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + if (gcs3.initialised) { + mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + } } From 21143d87b5f512bb3c462166df412ce50afc5e48 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 4 Dec 2011 06:32:08 +0800 Subject: [PATCH 03/14] add 1hz mavlink loop. --- ArduCopter/ArduCopter.pde | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b0b858037a..8cd529e4ff 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -913,8 +913,8 @@ static void slow_loop() update_lights(); // send all requested output streams with rates requested - // between 1 and 5 Hz - gcs_data_stream_send(1,5); + // between 3 and 5 Hz + gcs_data_stream_send(3,5); if(g.radio_tuning > 0) tuning(); @@ -947,6 +947,7 @@ static void super_slow_loop() Log_Write_Current(); gcs_send_message(MSG_HEARTBEAT); + gcs_data_stream_send(1,3); // agmatthews - USERHOOKS #ifdef USERHOOK_SUPERSLOWLOOP USERHOOK_SUPERSLOWLOOP From 8af8badcecd2a8b850e2c4ba8bd64ab5a129d1cb Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 15:28:09 -0800 Subject: [PATCH 04/14] formatting --- ArduCopter/GCS_Mavlink.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 1eef88ec3f..aef2542889 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1722,9 +1722,9 @@ static void mavlink_delay(unsigned long t) gcs_update(); } delay(1); -#if USB_MUX_PIN > 0 + #if USB_MUX_PIN > 0 check_usb_mux(); -#endif + #endif } while (millis() - tstart < t); in_mavlink_delay = false; From 06341cb1dc94223a54db0904bb52eae3c80efe00 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 15:28:36 -0800 Subject: [PATCH 05/14] Added Deg to rad, rad to deg * 100 to save a float calc --- ArduCopter/defines.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 63597b2a9c..117a33836f 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -334,7 +334,9 @@ enum gcs_severity { //#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters // RADIANS -#define RADX100 0.000174533 +#define RADX100 0.000174532925 +#define DEGX100 5729.57795 + // EEPROM addresses #define EEPROM_MAX_ADDR 4096 From 8e0d8a860b696db6e9c0c5ab69a251e4d4fbfc37 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 15:29:23 -0800 Subject: [PATCH 06/14] Formatting, swicth Omega to raw IMU rates --- ArduCopter/ArduCopter.pde | 81 +++++++++++++++++++-------------------- 1 file changed, 39 insertions(+), 42 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 8cd529e4ff..687751530e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -151,10 +151,10 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #if HIL_MODE == HIL_MODE_DISABLED - // real sensors - #if CONFIG_ADC == ENABLED +// real sensors +#if CONFIG_ADC == ENABLED AP_ADC_ADS7844 adc; - #endif +#endif #ifdef DESKTOP_BUILD APM_BMP085_HIL_Class barometer; @@ -164,35 +164,35 @@ static AP_Int8 *flight_modes = &g.flight_mode1; AP_Compass_HMC5843 compass(Parameters::k_param_compass); #endif - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; - #endif +#ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow; +#endif - // real GPS selection - #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO - AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); +// real GPS selection +#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO + AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); - #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA - AP_GPS_NMEA g_gps_driver(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA + AP_GPS_NMEA g_gps_driver(&Serial1); - #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF - AP_GPS_SIRF g_gps_driver(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF + AP_GPS_SIRF g_gps_driver(&Serial1); - #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX - AP_GPS_UBLOX g_gps_driver(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX + AP_GPS_UBLOX g_gps_driver(&Serial1); - #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK - AP_GPS_MTK g_gps_driver(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK + AP_GPS_MTK g_gps_driver(&Serial1); - #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 - AP_GPS_MTK16 g_gps_driver(&Serial1); +#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 + AP_GPS_MTK16 g_gps_driver(&Serial1); - #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE - AP_GPS_None g_gps_driver(NULL); +#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE + AP_GPS_None g_gps_driver(NULL); - #else - #error Unrecognised GPS_PROTOCOL setting. - #endif // GPS PROTOCOL +#else + #error Unrecognised GPS_PROTOCOL setting. +#endif // GPS PROTOCOL #if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); @@ -246,19 +246,16 @@ GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3); // ModeFilter sonar_mode_filter; #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 - -#if SONAR_TYPE == MAX_SONAR_XL - AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter); -#else - #error Unrecognised SONAR_TYPE setting. -#endif + #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 + #if SONAR_TYPE == MAX_SONAR_XL + AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter); + #else + #error Unrecognised SONAR_TYPE setting. + #endif #endif // agmatthews USERHOOKS @@ -560,7 +557,7 @@ static bool new_radio_frame; AP_Relay relay; #if USB_MUX_PIN > 0 -static bool usb_connected; + static bool usb_connected; #endif @@ -748,7 +745,7 @@ static void medium_loop() //------------------------------------------------- case 3: medium_loopCounter++; - // test + // perform next command // -------------------- if(control_mode == AUTO){ @@ -923,9 +920,9 @@ static void slow_loop() update_motor_leds(); #endif -#if USB_MUX_PIN > 0 + #if USB_MUX_PIN > 0 check_usb_mux(); -#endif + #endif break; default: @@ -1270,7 +1267,7 @@ static void read_AHRS(void) #endif dcm.update_DCM_fast(); - omega = dcm.get_gyro(); + omega = imu.get_gyro(); } static void update_trig(void){ From ffa69f43ece1b8d44feaab711bbeee1e4b9ffac2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 15:38:46 -0800 Subject: [PATCH 07/14] disabled Flash logs in 1280 by default Added stability patch I values and limits --- ArduCopter/config.h | 62 ++++++++++++++++++++------------------------- 1 file changed, 28 insertions(+), 34 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 60562b5080..fe2d26598a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -369,8 +369,6 @@ #ifndef THROTTLE_FAILSAFE_ACTION # define THROTTLE_FAILSAFE_ACTION 2 #endif - - #ifndef MINIMUM_THROTTLE # define MINIMUM_THROTTLE 130 #endif @@ -485,11 +483,11 @@ // and charachteristics changes. #ifdef MOTORS_JD880 # define STABILIZE_ROLL_P 3.6 -# define STABILIZE_ROLL_I 0.06 -# define STABILIZE_ROLL_IMAX 2.0 // degrees +# define STABILIZE_ROLL_I 0.08 +# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_PITCH_P 3.6 -# define STABILIZE_PITCH_I 0.06 -# define STABILIZE_PITCH_IMAX 2.0 // degrees +# define STABILIZE_PITCH_I 0.08 +# define STABILIZE_PITCH_IMAX 40.0 // degrees #endif // Jasons default values that are good for smaller payload motors. @@ -497,20 +495,20 @@ # define STABILIZE_ROLL_P 4.6 #endif #ifndef STABILIZE_ROLL_I -# define STABILIZE_ROLL_I 0.0 +# define STABILIZE_ROLL_I 0.08 #endif #ifndef STABILIZE_ROLL_IMAX -# define STABILIZE_ROLL_IMAX 1.5 // degrees +# define STABILIZE_ROLL_IMAX 40 // degrees #endif #ifndef STABILIZE_PITCH_P # define STABILIZE_PITCH_P 4.6 #endif #ifndef STABILIZE_PITCH_I -# define STABILIZE_PITCH_I 0.0 +# define STABILIZE_PITCH_I 0.08 #endif #ifndef STABILIZE_PITCH_IMAX -# define STABILIZE_PITCH_IMAX 1.5 // degrees +# define STABILIZE_PITCH_IMAX 40 // degrees #endif ////////////////////////////////////////////////////////////////////////////// @@ -583,15 +581,6 @@ #endif -////////////////////////////////////////////////////////////////////////////// -// Autopilot control limits -// -// how much to we pitch towards the target -#ifndef PITCH_MAX -# define PITCH_MAX 22 // degrees -#endif - - ////////////////////////////////////////////////////////////////////////////// // Navigation control gains // @@ -670,15 +659,20 @@ # define DEBUG_LEVEL SEVERITY_LOW #endif - ////////////////////////////////////////////////////////////////////////////// // Dataflash logging control // - -#ifndef LOGGING_ENABLED -# define LOGGING_ENABLED ENABLED +#ifdef LOGGING_ENABLED + #undef LOGGING_ENABLED #endif +#if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included + #define LOGGING_ENABLED ENABLED +#else + #define LOGGING_ENABLED DISABLED +#endif + + #ifndef LOG_ATTITUDE_FAST # define LOG_ATTITUDE_FAST DISABLED #endif @@ -723,17 +717,17 @@ #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) #define DEFAULT_LOG_BITMASK \ - LOGBIT(ATTITUDE_FAST) | \ - LOGBIT(ATTITUDE_MED) | \ - LOGBIT(GPS) | \ - LOGBIT(PM) | \ - LOGBIT(CTUN) | \ - LOGBIT(NTUN) | \ - LOGBIT(MODE) | \ - LOGBIT(RAW) | \ - LOGBIT(CMD) | \ - LOGBIT(CUR) | \ - LOGBIT(MOTORS) | \ + LOGBIT(ATTITUDE_FAST) | \ + LOGBIT(ATTITUDE_MED) | \ + LOGBIT(GPS) | \ + LOGBIT(PM) | \ + LOGBIT(CTUN) | \ + LOGBIT(NTUN) | \ + LOGBIT(MODE) | \ + LOGBIT(RAW) | \ + LOGBIT(CMD) | \ + LOGBIT(CUR) | \ + LOGBIT(MOTORS) | \ LOGBIT(OPTFLOW) // if we are using fast, Disable Medium From 3dc43c21af6c25157329f7bc27ab3841b228b971 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 15:39:02 -0800 Subject: [PATCH 08/14] Enabled the compass test --- ArduCopter/test.pde | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index e68a808c05..d0fd75e7f5 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -30,7 +30,7 @@ static int8_t test_current(uint8_t argc, const Menu::arg *argv); //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_baro(uint8_t argc, const Menu::arg *argv); -//static int8_t test_mag(uint8_t argc, const Menu::arg *argv); +static int8_t test_mag(uint8_t argc, const Menu::arg *argv); static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); #ifdef OPTFLOW_ENABLED static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); @@ -87,7 +87,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { #if CONFIG_SONAR == ENABLED {"sonar", test_sonar}, #endif - //{"compass", test_mag}, + {"compass", test_mag}, #ifdef OPTFLOW_ENABLED {"optflow", test_optflow}, #endif @@ -894,9 +894,9 @@ test_baro(uint8_t argc, const Menu::arg *argv) } #endif -/* + static int8_t -//test_mag(uint8_t argc, const Menu::arg *argv) +test_mag(uint8_t argc, const Menu::arg *argv) { if(g.compass_enabled) { //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); @@ -924,7 +924,7 @@ static int8_t return (0); } } -*/ + /* static int8_t //test_reverse(uint8_t argc, const Menu::arg *argv) From be3dba817b88fc3b2639dec637c749f52af40a47 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 17:54:38 -0800 Subject: [PATCH 09/14] Stability patch from 55 --- ArduCopter/Attitude.pde | 35 +++++++++++++++++++++++------------ 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 3aa78a1ba2..3aec0b6207 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -1,28 +1,34 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + static int get_stabilize_roll(int32_t target_angle) { int32_t error; int32_t rate; + // angle error error = wrap_180(target_angle - dcm.roll_sensor); // limit the error we're feeding to the PID error = constrain(error, -2500, 2500); - // desired Rate: + // conver to desired Rate: rate = g.pi_stabilize_roll.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); + + // experiment to pipe iterm directly into the output + int16_t iterm = g.pi_stabilize_roll.get_integrator(); + + // remove iterm from PI output + rate -= iterm; #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters - // Rate P: - error = rate - (degrees(omega.x) * 100.0); + error = rate - (omega.x * DEGX100); rate = g.pi_rate_roll.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif // output control: - return (int)constrain(rate, -2500, 2500); + rate = constrain(rate, -2500, 2500); + return (int)rate + iterm; } static int @@ -31,24 +37,29 @@ get_stabilize_pitch(int32_t target_angle) int32_t error; int32_t rate; + // angle error error = wrap_180(target_angle - dcm.pitch_sensor); // limit the error we're feeding to the PID error = constrain(error, -2500, 2500); - // desired Rate: + // conver to desired Rate: rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); + + // experiment to pipe iterm directly into the output + int16_t iterm = g.pi_stabilize_roll.get_integrator(); + + // remove iterm from PI output + rate -= iterm; #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters - // Rate P: - error = rate - (degrees(omega.y) * 100.0); + error = rate - (omega.y * DEGX100); rate = g.pi_rate_pitch.get_pi(error, G_Dt); - //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif // output control: - return (int)constrain(rate, -2500, 2500); + rate = constrain(rate, -2500, 2500); + return (int)rate + iterm; } From fdcf9aecd5e90424fa63b83db8df6ecd76eddac6 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 17:54:57 -0800 Subject: [PATCH 10/14] Fixing reversed Simple mode switch --- ArduCopter/control_modes.pde | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 94b6f7e3be..356a2158a1 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -14,9 +14,9 @@ static void read_control_switch() set_mode(flight_modes[switchPosition]); - if(g.ch7_option == CH7_SIMPLE_MODE){ - // setup Simple mode - // do we enable simple mode? + if(g.ch7_option != CH7_SIMPLE_MODE){ + // set Simple mode using stored paramters from Mission planner + // rather than by the control switch do_simple = (g.simple_modes & (1 << switchPosition)); } }else{ From 11363a26cbd465963c2723e9f08b367a7b10ba7f Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 17:55:11 -0800 Subject: [PATCH 11/14] Added new gain and clamp value for DCM --- ArduCopter/system.pde | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 5c07a92502..c3ea4c41e4 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -343,10 +343,11 @@ static void init_ardupilot() reset_control_switch(); #if HIL_MODE != HIL_MODE_ATTITUDE - dcm.kp_roll_pitch(0.030000); + dcm.kp_roll_pitch(0.130000); dcm.ki_roll_pitch(0.00001278), // 50 hz I term dcm.kp_yaw(0.08); dcm.ki_yaw(0.00004); + dcm._clamp = 5; #endif // init the Z damopener From 50afa16f3133e2d025e4a500ea8c0a953b911691 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 17:55:44 -0800 Subject: [PATCH 12/14] Function call defs commented with // so the preprocessor doesn't get confused --- ArduCopter/test.pde | 33 +++++++++++++++------------------ 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index d0fd75e7f5..d0dc857c35 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -125,7 +125,8 @@ test_eedump(uint8_t argc, const Menu::arg *argv) return(0); } -/*static int8_t +/* +//static int8_t //test_radio_pwm(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); @@ -157,7 +158,8 @@ test_eedump(uint8_t argc, const Menu::arg *argv) } }*/ -/*static int8_t +/* +//static int8_t //test_tri(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); @@ -185,7 +187,7 @@ test_eedump(uint8_t argc, const Menu::arg *argv) }*/ /* -static int8_t +//static int8_t //test_nav(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); @@ -258,7 +260,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) } /* -static int8_t +//static int8_t //test_failsafe(uint8_t argc, const Menu::arg *argv) { @@ -313,7 +315,8 @@ static int8_t } */ -/*static int8_t +/* +//static int8_t //test_stabilize(uint8_t argc, const Menu::arg *argv) { static byte ts_num; @@ -404,7 +407,7 @@ static int8_t /*#if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_ADC == ENABLED -static int8_t +//static int8_t //test_adc(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); @@ -538,12 +541,6 @@ test_dcm_eulers(uint8_t argc, const Menu::arg *argv) // IMU // --- read_AHRS(); - - //Vector3f accels = imu.get_accel(); - //Vector3f gyros = imu.get_gyro(); - //Vector3f accel_filt = imu.get_accel_filtered(); - //accels_rot = dcm.get_dcm_matrix() * accel_filt; - medium_loopCounter++; if(medium_loopCounter == 4){ @@ -606,8 +603,8 @@ test_gps(uint8_t argc, const Menu::arg *argv) } /* -static int8_t -test_dcm(uint8_t argc, const Menu::arg *argv) +//static int8_t +//test_dcm(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); delay(1000); @@ -657,8 +654,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv) } */ /* -static int8_t -test_dcm(uint8_t argc, const Menu::arg *argv) +//static int8_t +//test_dcm(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); delay(1000); @@ -681,7 +678,7 @@ test_dcm(uint8_t argc, const Menu::arg *argv) */ /*static int8_t -test_omega(uint8_t argc, const Menu::arg *argv) +//test_omega(uint8_t argc, const Menu::arg *argv) { static byte ts_num; float old_yaw; @@ -782,7 +779,7 @@ test_current(uint8_t argc, const Menu::arg *argv) } /* -static int8_t +//static int8_t //test_relay(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); From d54d9b618cfe44298d2fbde97644438532a95f94 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 17:56:41 -0800 Subject: [PATCH 13/14] Added faster conversion from and to degrees*100 / rads --- libraries/AP_DCM/AP_DCM.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 751caa00b2..95a11c5bb4 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -1,3 +1,6 @@ + +#define RADX100 0.000174532925 +#define DEGX100 5729.57795 /* APM_DCM_FW.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega Code by Doug Weibel, Jordi Muņoz and Jose Julio. DIYDrones.com @@ -274,7 +277,7 @@ AP_DCM::drift_correction(void) // Dynamic weighting of accelerometer info (reliability filter) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - accel_weight = constrain(1 - 3 * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0) + accel_weight = constrain(1 - _clamp * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0) // We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting _health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1); @@ -346,7 +349,7 @@ AP_DCM::drift_correction(void) // Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second integrator_magnitude = _omega_I.length(); if (integrator_magnitude > radians(30)) { - _omega_I *= (radians(30) / integrator_magnitude); + _omega_I *= (radians(30) / integrator_magnitude); } //Serial.print("*"); } @@ -379,18 +382,16 @@ AP_DCM::euler_rp(void) { pitch = -asin(_dcm_matrix.c.x); roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z); - roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; + roll_sensor = roll * DEGX100; //degrees(roll) * 100; + pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100; } void AP_DCM::euler_yaw(void) { yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); - yaw_sensor = degrees(yaw) * 100; + yaw_sensor = yaw * DEGX100; //degrees(yaw) * 100; if (yaw_sensor < 0) yaw_sensor += 36000; } - - From 17fbbb4e7bae50bee83ef36dde2f4687422872c2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 17:57:12 -0800 Subject: [PATCH 14/14] Access to _clamp val. May want to make a private member and write accessor. --- libraries/AP_DCM/AP_DCM.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index afec0fcd45..0320539e69 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -35,7 +35,8 @@ public: _ki_roll_pitch(0.00001278), _kp_yaw(0.8), // .8 _ki_yaw(0.00004), // 0.00004 - _toggle(0) + _toggle(0), + _clamp(3) {} // Accessors @@ -44,7 +45,7 @@ public: Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} Vector3f get_integrator(void) {return _omega_I; } // We return the current drift correction integrator values - + float get_health(void) {return _health;} void set_centripetal(bool b) {_centripetal = b;} bool get_centripetal(void) {return _centripetal;} @@ -81,6 +82,7 @@ public: static const float kDCM_kp_rp_high = 0.15; static const float kDCM_kp_rp_medium = 0.05967; static const float kDCM_kp_rp_low = 0.01; + int8_t _clamp; private: