From 1f46609eca92a8e15da3031203c8dd602572c729 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 13 Feb 2011 05:21:32 +0000 Subject: [PATCH] alt hold updates git-svn-id: https://arducopter.googlecode.com/svn/trunk@1633 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 31 +++++++------------------------ ArduCopterMega/commands.pde | 8 +++++--- ArduCopterMega/config.h | 4 ++-- ArduCopterMega/flight_control.pde | 9 +++++++-- ArduCopterMega/motors.pde | 10 ++++++---- ArduCopterMega/navigation.pde | 4 ++++ ArduCopterMega/system.pde | 4 +--- ArduCopterMega/test.pde | 3 ++- 8 files changed, 34 insertions(+), 39 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 8f2304fa60..66cfa464a5 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -298,6 +298,7 @@ long nav_yaw; // deg * 100 : target yaw angle long nav_lat; // for error calcs long nav_lon; // for error calcs int nav_throttle; // 0-1000 for throttle control +int nav_throttle_old; // 0-1000 for throttle control long command_yaw_start; // what angle were we to begin with long command_yaw_start_time; // when did we start turning @@ -708,6 +709,7 @@ void update_GPS(void) GPS.update(); update_GPS_light(); + // !!! comment out after testing //fake_out_gps(); if (GPS.new_data && GPS.fix) { @@ -768,11 +770,6 @@ void update_current_flight_mode(void) // break; default: - // Intput Pitch, Roll, Yaw and Throttle - // ------------------------------------ - //calc_nav_pid(); - //calc_nav_roll(); - //calc_nav_pitch(); // based on altitude error // ----------------------- @@ -798,8 +795,6 @@ void update_current_flight_mode(void) switch(control_mode){ case ACRO: - // Intput Pitch, Roll, Yaw and Throttle - // ------------------------------------ // clear any AP naviagtion values nav_pitch = 0; nav_roll = 0; @@ -835,8 +830,6 @@ void update_current_flight_mode(void) break; case STABILIZE: - // Intput Pitch, Roll, Yaw and Throttle - // ------------------------------------ // clear any AP naviagtion values nav_pitch = 0; nav_roll = 0; @@ -903,8 +896,6 @@ void update_current_flight_mode(void) break; case ALT_HOLD: - // Intput Pitch, Roll, Yaw and Throttle - // ------------------------------------ // clear any AP naviagtion values nav_pitch = 0; nav_roll = 0; @@ -913,6 +904,9 @@ void update_current_flight_mode(void) // get desired height from the throttle next_WP.alt = home.alt + (rc_3.control_in * 4); // 0 - 1000 (40 meters) + // !!! testing + //next_WP.alt -= 500; + // Yaw control // ----------- output_manual_yaw(); @@ -934,13 +928,7 @@ void update_current_flight_mode(void) output_stabilize_pitch(); break; - case RTL: - // Intput Pitch, Roll, Yaw and Throttle - // ------------------------------------ - //calc_nav_pid(); - //calc_nav_roll(); - //calc_nav_pitch(); - + case RTL: // based on altitude error // ----------------------- calc_nav_throttle(); @@ -961,12 +949,7 @@ void update_current_flight_mode(void) break; case POSITION_HOLD: - // Intput Pitch, Roll, Yaw and Throttle - // ------------------------------------ - //calc_nav_pid(); - //calc_nav_roll(); - //calc_nav_pitch(); - + // Yaw control // ----------- output_manual_yaw(); diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 40ca6620c5..1c9e977ef1 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -133,7 +133,9 @@ void set_current_loc_here() { //struct Location temp; - set_next_WP(¤t_loc); + Location l = current_loc; + l.alt = get_altitude_above_home(); + set_next_WP(&l); } /* @@ -160,8 +162,8 @@ set_next_WP(struct Location *wp) // used to control FBW and limit the rate of climb // ----------------------------------------------- - target_altitude = current_loc.alt; - offset_altitude = next_WP.alt - current_loc.alt; + target_altitude = current_loc.alt; // PH: target_altitude = 200 + offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0 // zero out our loiter vals to watch for missed waypoints loiter_delta = 0; diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 77242e33be..6ac7ff5557 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -386,10 +386,10 @@ # define THROTTLE_BARO_I 0.05 #endif #ifndef THROTTLE_BARO_D -# define THROTTLE_BARO_D 0.06 +# define THROTTLE_BARO_D 0.2 #endif #ifndef THROTTLE_BARO_IMAX -# define THROTTLE_BARO_IMAX 100 +# define THROTTLE_BARO_IMAX 50 #endif #ifndef THROTTLE_SONAR_P diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index ef3e50a3ef..cc327f954f 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -28,13 +28,15 @@ void calc_nav_throttle() float t = pid_baro_throttle.kP(); if(error > 0){ - //pid_baro_throttle.kP(.25); + pid_baro_throttle.kP(t); }else{ pid_baro_throttle.kP(t/4.0); } + // limit output of throttle control nav_throttle = pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0); - nav_throttle = throttle_cruise + constrain(nav_throttle, -20, 70); + nav_throttle = throttle_cruise + constrain(nav_throttle, -30, 80); + pid_baro_throttle.kP(t); } else { @@ -44,6 +46,9 @@ void calc_nav_throttle() // limit output of throttle control nav_throttle = throttle_cruise + constrain(nav_throttle, -60, 100); } + + nav_throttle = (nav_throttle + nav_throttle_old) >> 1; + nav_throttle_old = nav_throttle; } float angle_boost() diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 89203d243b..278e495eb4 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -182,6 +182,7 @@ set_servos_4() write_int(motor_out[CH_3]); write_int(motor_out[CH_4]); + write_int(rc_3.servo_out); write_int((int)(cos_yaw_x * 100)); write_int((int)(sin_yaw_y * 100)); write_int((int)(dcm.yaw_sensor / 100)); @@ -192,11 +193,12 @@ set_servos_4() write_int((int)nav_roll); write_int((int)nav_pitch); + //24 - write_long(home.lat); //28 - write_long(home.lng); //32 - write_int((int)home.alt); //34 - + write_long(current_loc.lat); //28 + write_long(current_loc.lng); //32 + write_int((int)current_loc.alt); //34 + write_long(next_WP.lat); write_long(next_WP.lng); write_int((int)next_WP.alt); //44 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index b645f54625..7a1abb94c0 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -149,6 +149,10 @@ void calc_altitude_error() //Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude); } +// target_altitude = current_loc.alt; // PH: target_altitude = -200 +// offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0 + + long wrap_360(long error) { if (error > 36000) error -= 36000; diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 058631d639..7e54b09189 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -258,9 +258,7 @@ void set_mode(byte mode) control_mode = mode; control_mode = constrain(control_mode, 0, NUM_MODES - 1); - - save_EEPROM_PID(); - + // used to stop fly_aways if(rc_1.control_in == 0){ // we are on the ground is this is true diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 1330ead4c7..38b00ae674 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -868,7 +868,8 @@ void fake_out_gps() GPS.latitude = 377696000; // Y GPS.longitude = -1224319000; // X - + GPS.altitude = 9000; // meters * 100 + //next_WP.lng = home.lng - length * sin(rads); // X //next_WP.lat = home.lat + length * cos(rads); // Y }