diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index eae295514f..4f5574ab86 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -40,7 +40,7 @@ CH7_ADC_FILTER (experimental) */ -#define ACCEL_ALT_HOLD 0 +#define ACCEL_ALT_HOLD 0 // disabled by default, work in progress #define ACCEL_ALT_HOLD_GAIN 12.0 // ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ba70d3791a..04a1774322 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -260,16 +260,20 @@ static const char* flight_mode_strings[] = { */ // test +#if ACCEL_ALT_HOLD == 1 Vector3f accels_rot; -//float accel_gain = 12; +static int accels_rot_count; +static float accels_rot_sum; +static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN; +#endif // temp -int y_actual_speed; -int y_rate_error; +static int y_actual_speed; +static int y_rate_error; // calc the -int x_actual_speed; -int x_rate_error; +static int x_actual_speed; +static int x_rate_error; // Radio // ----- @@ -585,7 +589,7 @@ static void fast_loop() set_servos_4(); //if(motor_armed) - // Log_Write_Attitude(); + //Log_Write_Attitude(); } static void medium_loop() @@ -1078,7 +1082,7 @@ void update_throttle_mode(void) altitude_error = get_altitude_error(); // get the AP throttle - nav_throttle = get_nav_throttle(altitude_error, 250); //150 = target speed of 1.5m/s + nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); // clear the new data flag @@ -1201,8 +1205,12 @@ static void update_trig(void){ // 270 = cos_yaw: -1.00, sin_yaw: 0.00, + #if ACCEL_ALT_HOLD == 1 Vector3f accel_filt = imu.get_accel_filtered(); accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); + accels_rot_sum += accels_rot.z; + accels_rot_count++; + #endif } // updated at 10hz @@ -1266,6 +1274,13 @@ static void tuning(){ tuning_value = (float)g.rc_6.control_in / 1000.0; switch(g.radio_tuning){ + + /*case CH6_STABILIZE_KP: + g.rc_6.set_range(0,2000); // 0 to 8 + tuning_value = (float)g.rc_6.control_in / 100.0; + alt_hold_gain = tuning_value; + break;*/ + case CH6_STABILIZE_KP: g.rc_6.set_range(0,8000); // 0 to 8 g.pi_stabilize_roll.kP(tuning_value); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index a91b5ce4a6..dd00de1c08 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -83,19 +83,18 @@ get_stabilize_yaw(long target_angle) return (int)constrain(rate, -2500, 2500); } -#define ALT_ERROR_MAX 300 +#define ALT_ERROR_MAX 400 static int -get_nav_throttle(long z_error, int target_speed) +get_nav_throttle(long z_error)//, //int target_speed) { - int rate_error; - float scaler = (float)target_speed/(float)ALT_ERROR_MAX; - // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); - target_speed = z_error * scaler; - rate_error = target_speed - altitude_rate; - rate_error = constrain(rate_error, -110, 110); + int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 + rate_error = rate_error - altitude_rate; + + // limit the rate + rate_error = constrain(rate_error, -100, 120); return (int)g.pi_throttle.get_pi(rate_error, .1); } @@ -187,20 +186,35 @@ get_nav_yaw_offset(int yaw_input, int reset) } } -///* static int alt_hold_velocity() { #if ACCEL_ALT_HOLD == 1 // subtract filtered Accel float error = abs(next_WP.alt - current_loc.alt); + + error -= 100; error = min(error, 200.0); + error = max(error, 0.0); error = 1 - (error/ 200.0); - return (accels_rot.z + 9.81) * ACCEL_ALT_HOLD_GAIN * error; + float sum = accels_rot_sum / (float)accels_rot_count; + + accels_rot_sum = 0; + accels_rot_count = 0; + + int output = (sum + 9.81) * alt_hold_gain * error; + +// fast rise +//s: -17.6241, g:0.0000, e:1.0000, o:0 +//s: -18.4990, g:0.0000, e:1.0000, o:0 +//s: -19.3193, g:0.0000, e:1.0000, o:0 +//s: -13.1310, g:47.8700, e:1.0000, o:-158 + + //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); + return output; #else return 0; #endif } -//*/ static int get_angle_boost() { diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b520c6871e..8b1ce7fff3 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -164,6 +164,7 @@ public: k_param_pi_loiter_lon, k_param_pi_nav_lat, k_param_pi_nav_lon, + k_param_pi_alt_hold, k_param_pi_throttle, k_param_pi_crosstrack, @@ -277,6 +278,7 @@ public: APM_PI pi_nav_lat; APM_PI pi_nav_lon; + APM_PI pi_alt_hold; APM_PI pi_throttle; APM_PI pi_crosstrack; @@ -381,7 +383,8 @@ public: pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100), pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100), - pi_throttle (k_param_pi_throttle, PSTR("THR_HOLD_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), + pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), THR_HOLD_P, THR_HOLD_I, THR_HOLD_IMAX), + pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), pi_crosstrack (k_param_pi_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_IMAX), junk(0) // XXX just so that we can add things without worrying about the trailing comma diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 49c6bb941c..e381534a47 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -482,7 +482,7 @@ static void do_wait_delay() //Serial.print("dwd "); condition_start = millis(); condition_value = next_command.lat * 1000; // convert to milliseconds - Serial.println(condition_value,DEC); + //Serial.println(condition_value,DEC); } static void do_change_alt() diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 6671526176..a756a222ad 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -388,7 +388,7 @@ // Attitude Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4.0 +# define STABILIZE_ROLL_P 4.6 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.001 @@ -398,7 +398,7 @@ #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 4.0 +# define STABILIZE_PITCH_P 4.6 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.001 @@ -411,7 +411,7 @@ // Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P .13 +# define RATE_ROLL_P 0.145 #endif #ifndef RATE_ROLL_I # define RATE_ROLL_I 0.0 @@ -421,7 +421,7 @@ #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.13 +# define RATE_PITCH_P 0.145 #endif #ifndef RATE_PITCH_I # define RATE_PITCH_I 0 //0.18 @@ -500,6 +500,16 @@ # define THROTTLE_CRUISE 350 // #endif +#ifndef THR_HOLD_P +# define THR_HOLD_P 0.80 // +#endif +#ifndef THR_HOLD_I +# define THR_HOLD_I 0.00 // with 4m error, 12.5s windup +#endif +#ifndef THR_HOLD_IMAX +# define THR_HOLD_IMAX 00 +#endif + #ifndef THROTTLE_P # define THROTTLE_P 0.6 // #endif @@ -507,7 +517,7 @@ # define THROTTLE_I 0.10 // with 4m error, 12.5s windup #endif #ifndef THROTTLE_IMAX -# define THROTTLE_IMAX 300 +# define THROTTLE_IMAX 300 #endif diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 394fc404d5..ba03570201 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -120,7 +120,7 @@ static void auto_trim() led_mode = NORMAL_LEDS; clear_leds(); imu.save(); - Serial.println("Done"); + //Serial.println("Done"); auto_level_counter = 0; } } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index f396bfe74f..a075f10a29 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -24,7 +24,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); @@ -76,11 +76,11 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"altitude", test_baro}, #endif {"sonar", test_sonar}, - {"compass", test_mag}, + //{"compass", test_mag}, #ifdef OPTFLOW_ENABLED {"optflow", test_optflow}, #endif - {"xbee", test_xbee}, + //{"xbee", test_xbee}, {"eedump", test_eedump}, {"rawgps", test_rawgps}, // {"mission", test_mission}, @@ -769,7 +769,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) { } } -static int8_t +/*static int8_t test_xbee(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); @@ -785,6 +785,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv) } } } +*/ #if HIL_MODE != HIL_MODE_ATTITUDE static int8_t @@ -805,6 +806,7 @@ test_baro(uint8_t argc, const Menu::arg *argv) } #endif +/* static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { @@ -834,6 +836,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) return (0); } } +*/ /* static int8_t test_reverse(uint8_t argc, const Menu::arg *argv)