From 77966c4c6a4e0a875b2894cd9545f877d312fc5f Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 29 May 2011 19:32:55 +0000 Subject: [PATCH] Better sonar hold ++ added mag calibration - still not sure if needed. better sonar spike filter 2.0.19 git-svn-id: https://arducopter.googlecode.com/svn/trunk@2434 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 2 +- ArduCopterMega/ArduCopterMega.pde | 10 +++--- ArduCopterMega/Attitude.pde | 6 ++-- ArduCopterMega/GCS.h | 6 ++-- ArduCopterMega/GCS_Mavlink.pde | 54 ++++++++++++++++--------------- ArduCopterMega/Log.pde | 10 +++--- ArduCopterMega/config.h | 10 +++--- ArduCopterMega/setup.pde | 1 + ArduCopterMega/system.pde | 2 +- 9 files changed, 52 insertions(+), 49 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 55d0633597..2eba1a6e50 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -7,7 +7,7 @@ //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #define NAV_TEST 1 // 0 = traditional, 1 = rate controlled -#define YAW_OPTION 0 // 0 = hybrid rate approach, 1 = offset Yaw approach +#define YAW_OPTION 1 // 0 = hybrid rate approach, 1 = offset Yaw approach #define AUTO_RESET_LOITER 0 // enables Loiter to reset it's current location based on stick input. #define FRAME_CONFIG QUAD_FRAME diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 1d40eb7c91..58c9712c9d 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -642,7 +642,7 @@ void medium_loop() if (g.log_bitmask & MASK_LOG_NTUN) Log_Write_Nav_Tuning(); - if (g.log_bitmask & MASK_LOG_GPS){ + if (GPS_enabled && g.log_bitmask & MASK_LOG_GPS){ Log_Write_GPS(); } @@ -1230,13 +1230,11 @@ void update_alt() // filter out offset // read barometer - baro_alt = read_barometer(); - - // XXX temp removed fr debugging - //filter out bad sonar reads + baro_alt = read_barometer(); int temp_sonar = sonar.read(); - if(abs(temp_sonar - sonar_alt) < 300){ + // spike filter + if((temp_sonar - sonar_alt) < 50){ sonar_alt = temp_sonar; } diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 25a8a2dfc3..ac8dad4c83 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -132,15 +132,15 @@ void calc_nav_throttle() if(error < 0){ // try and prevent rapid fall - //scaler = (altitude_sensor == BARO) ? 1 : 1; + scaler = (altitude_sensor == BARO) ? .8 : .8; } if(altitude_sensor == BARO){ nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_medium_loop, scaler); // .2 - nav_throttle = g.throttle_cruise + constrain(nav_throttle, -35, 80); + nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80); }else{ nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_medium_loop, scaler); // .5 - nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 100); + nav_throttle = g.throttle_cruise + constrain(nav_throttle, -40, 100); } // simple filtering diff --git a/ArduCopterMega/GCS.h b/ArduCopterMega/GCS.h index 940522986f..8dff8c6475 100644 --- a/ArduCopterMega/GCS.h +++ b/ArduCopterMega/GCS.h @@ -110,8 +110,10 @@ public: // used by data_stream_send static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax) { - if (freq != 0 && freq >= freqMin && freq < freqMax) return true; - else return false; + if (freq != 0 && freq >= freqMin && freq < freqMax) + return true; + else + return false; } // send streams which match frequency range diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 86779fdd7f..13ede51a2c 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -87,40 +87,42 @@ void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { - if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax)) - send_message(MSG_RAW_IMU); - if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) { - send_message(MSG_EXTENDED_STATUS); - send_message(MSG_GPS_STATUS); - send_message(MSG_CURRENT_WAYPOINT); - send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working + if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ + send_message(MSG_RAW_IMU); + } + + if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { + send_message(MSG_EXTENDED_STATUS); + send_message(MSG_GPS_STATUS); + send_message(MSG_CURRENT_WAYPOINT); + send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); - } + } - if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) { - send_message(MSG_LOCATION); - } + if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { + send_message(MSG_LOCATION); + } - if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) { - // This is used for HIL. Do not change without discussing with HIL maintainers - send_message(MSG_SERVO_OUT); - } + if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { + // This is used for HIL. Do not change without discussing with HIL maintainers + send_message(MSG_SERVO_OUT); + } - if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) { - send_message(MSG_RADIO_OUT); - send_message(MSG_RADIO_IN); - } + if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { + send_message(MSG_RADIO_OUT); + send_message(MSG_RADIO_IN); + } - if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info - send_message(MSG_ATTITUDE); - } + if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info + send_message(MSG_ATTITUDE); + } - if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info - send_message(MSG_VFR_HUD); - } + if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info + send_message(MSG_VFR_HUD); + } - if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){ + if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ // Available datastream } } diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 8136e5572b..50935c010c 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -482,15 +482,15 @@ void Log_Write_Control_Tuning() DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); // Control - DataFlash.WriteInt((int)(g.rc_4.control_in)); - DataFlash.WriteInt((int)(g.rc_4.servo_out)); + DataFlash.WriteInt((int)(g.rc_4.control_in/100)); + DataFlash.WriteInt((int)(g.rc_4.servo_out/100)); // yaw DataFlash.WriteByte(yaw_debug); DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); DataFlash.WriteInt((int)(nav_yaw/100)); DataFlash.WriteInt((int)yaw_error/100); - DataFlash.WriteInt((int)(omega.z * 1000)); + DataFlash.WriteInt((int)(omega.z * 100)); // Alt hold DataFlash.WriteInt((int)(g.rc_3.servo_out)); @@ -499,7 +499,7 @@ void Log_Write_Control_Tuning() DataFlash.WriteInt((int)next_WP.alt); // DataFlash.WriteInt((int)altitude_error); // - DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator()); + DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator()); DataFlash.WriteByte(END_BYTE); } @@ -521,7 +521,7 @@ void Log_Read_Control_Tuning() DataFlash.ReadInt(), DataFlash.ReadInt(), DataFlash.ReadInt(), - (float)DataFlash.ReadInt() / 1000.0,// Gyro Rate + (float)DataFlash.ReadInt(),// Gyro Rate // Alt Hold DataFlash.ReadInt(), diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index a3ab05800d..e1ec6ac9b8 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -360,7 +360,7 @@ # define YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance #endif #ifndef YAW_D -# define YAW_D 0.05 // Trying a lower value to prevent odd behavior +# define YAW_D 0.08 // .7 = almost no yaw Trying a lower value to prevent odd behavior #endif #ifndef YAW_IMAX # define YAW_IMAX 1 // degrees * 100 @@ -438,16 +438,16 @@ #ifndef THROTTLE_SONAR_P -# define THROTTLE_SONAR_P 0.5 // +# define THROTTLE_SONAR_P 0.35 // lowering P by .15 #endif #ifndef THROTTLE_SONAR_I -# define THROTTLE_SONAR_I 0.1 +# define THROTTLE_SONAR_I 0.05 #endif #ifndef THROTTLE_SONAR_D -# define THROTTLE_SONAR_D 0.06 +# define THROTTLE_SONAR_D 0.3 // increasing D by .5 #endif #ifndef THROTTLE_SONAR_IMAX -# define THROTTLE_SONAR_IMAX 60 +# define THROTTLE_SONAR_IMAX 20 #endif diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 17b925a0d3..ca8be4a96b 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -409,6 +409,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) if (!strcmp_P(argv[1].str, PSTR("c"))) { compass.set_offsets(_offsets); + compass.save_offsets(); report_compass(); return (0); } diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 1d322905f6..889ff85023 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.18 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.19 Beta", main_menu_commands); void init_ardupilot() {