diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4aa9ecadd4..2279a72a08 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.2 b4" +#define THISFIRMWARE "ArduCopter V2.2 b6" /* ArduCopter Version 2.2 Authors: Jason Short @@ -69,7 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // TimerProcess is the scheduler for MPU6000 reads. #include // ArduPilot Mega DCM Library #include // PI library -#include // PID library +#include // PID library #include // RC Channel Library #include // Range finder library #include // Optical Flow library @@ -312,11 +312,6 @@ static const char* flight_mode_strings[] = { //////////////////////////////////////////////////////////////////////////////// // The GPS based velocity calculated by offsetting the Latitude and Longitude // updated after GPS read - 5-10hz -static int16_t x_GPS_speed; -static int16_t y_GPS_speed; - -// The synthesized velocity calculated by fancy filtering and fusion -// updated at 50hz static int16_t x_actual_speed; static int16_t y_actual_speed; @@ -466,6 +461,8 @@ static uint8_t wp_verify_byte; // used for tracking state of navigating wa static int16_t waypoint_speed_gov; // Used to track how many cm we are from the "next_WP" location static int32_t long_error, lat_error; +// Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s +static boolean loiter_override; //////////////////////////////////////////////////////////////////////////////// @@ -554,19 +551,16 @@ static int32_t altitude_error; static int16_t climb_rate; // The altitude as reported by Sonar in cm – Values are 20 to 700 generally. static int16_t sonar_alt; -// The previous altitude as reported by Sonar in cm for calculation of Climb Rate -static int16_t old_sonar_alt; // The climb_rate as reported by sonar in cm/s static int16_t sonar_rate; // The altitude as reported by Baro in cm – Values can be quite high static int32_t baro_alt; -// The previous altitude as reported by Baro in cm for calculation of Climb Rate -static int32_t old_baro_alt; // The climb_rate as reported by Baro in cm/s static int16_t baro_rate; // static boolean reset_throttle_flag; + //////////////////////////////////////////////////////////////////////////////// // flight modes //////////////////////////////////////////////////////////////////////////////// @@ -598,6 +592,10 @@ static int16_t manual_boost; static int16_t angle_boost; // Push copter down for clean landing static int16_t landing_boost; +// for controlling the landing throttle curve +//verifies landings +static int16_t ground_detector; + //////////////////////////////////////////////////////////////////////////////// // Navigation general @@ -823,7 +821,7 @@ void loop() uint32_t timer = micros(); // We want this to execute fast // ---------------------------- - if ((timer - fast_loopTimer) >= 5000) { + if ((timer - fast_loopTimer) >= 4500) { //PORTK |= B00010000; G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops fast_loopTimer = timer; @@ -849,7 +847,7 @@ void loop() // update our velocity estimate based on IMU at 50hz // ------------------------------------------------- - estimate_velocity(); + //estimate_velocity(); // check for new GPS messages // -------------------------- @@ -900,6 +898,12 @@ static void fast_loop() // IMU DCM Algorithm read_AHRS(); + if(takeoff_complete == false){ + // reset these I terms to prevent awkward tipping on takeoff + reset_rate_I(); + reset_stability_I(); + } + // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -1515,11 +1519,6 @@ void update_throttle_mode(void) if ((millis() - takeoff_timer) > 5000){ // we must be in the air by now takeoff_complete = true; - land_complete = false; - }else{ - // reset these I terms to prevent awkward tipping on takeoff - reset_rate_I(); - reset_stability_I(); } }else{ // we are on the ground @@ -1532,11 +1531,6 @@ void update_throttle_mode(void) // reset_baro(); } - // reset out i terms and takeoff timer - // ----------------------------------- - reset_rate_I(); - reset_stability_I(); - // remember our time since takeoff // ------------------------------- takeoff_timer = millis(); @@ -1593,12 +1587,8 @@ void update_throttle_mode(void) // how far off are we altitude_error = get_altitude_error(); - // get the AP throttle, if landing boost > 0 we are actually Landing - // This allows us to grab just the compensation. - if(landing_boost > 0) - nav_throttle = get_nav_throttle(-200); - else - nav_throttle = get_nav_throttle(altitude_error); + // get the AP throttle + nav_throttle = get_nav_throttle(altitude_error); // clear the new data flag invalid_throttle = false; @@ -1612,6 +1602,11 @@ void update_throttle_mode(void) //*/ } + // hack to remove the influence of the ground effect + if(current_loc.alt < 200 && landing_boost != 0) { + nav_throttle = min(nav_throttle, 0); + } + #if FRAME_CONFIG == HELI_FRAME throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost); #else @@ -1659,24 +1654,18 @@ static void update_navigation() case RTL: // We have reached Home if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ - // if this value > 0, we are set to trigger auto_land after 30 seconds + // if auto_land_timer value > 0, we are set to trigger auto_land after 20 seconds set_mode(LOITER); auto_land_timer = millis(); break; } - // We wait until we've reached out new altitude before coming home - // Arg doesn't work, it - //if(alt_change_flag != REACHED_ALT){ - // wp_control = NO_NAV_MODE; - //}else{ - wp_control = WP_MODE; + wp_control = WP_MODE; - // calculates desired Yaw - #if FRAME_CONFIG == HELI_FRAME - update_auto_yaw(); - #endif - //} + // calculates desired Yaw + #if FRAME_CONFIG == HELI_FRAME + update_auto_yaw(); + #endif // calculates the desired Roll and Pitch update_nav_wp(); @@ -1687,7 +1676,15 @@ static void update_navigation() case POSITION: // This feature allows us to reposition the quad when the user lets // go of the sticks + if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ + // flag will reset when speed drops below .5 m/s + loiter_override = true; + } + + // Allow the user to take control temporarily, + // regain hold when the speed goes down to .5m/s + if(loiter_override){ // this sets the copter to not try and nav while we control it wp_control = NO_NAV_MODE; @@ -1695,8 +1692,11 @@ static void update_navigation() next_WP.lat = current_loc.lat; next_WP.lng = current_loc.lng; + if(g_gps->ground_speed < 50){ + loiter_override = false; + wp_control = LOITER_MODE; + } }else{ - // this is also set by GPS in update_nav wp_control = LOITER_MODE; } @@ -1712,7 +1712,10 @@ static void update_navigation() break; case LAND: - verify_land(); + if(g.sonar_enabled) + verify_land_sonar(); + else + verify_land_baro(); // calculates the desired Roll and Pitch update_nav_wp(); @@ -1795,6 +1798,10 @@ static void update_trig(void){ // updated at 10hz static void update_altitude() { + static int16_t old_sonar_alt = 0; + static int32_t old_baro_alt = 0; + static int16_t old_climb_rate = 0; + #if HIL_MODE == HIL_MODE_ATTITUDE // we are in the SIM, fake out the baro and Sonar int fake_relative_alt = g_gps->altitude - gps_base_alt; @@ -1815,7 +1822,7 @@ static void update_altitude() baro_rate = (temp + baro_rate) >> 1; old_baro_alt = baro_alt; - // sonar_alt is calculated in a faster loop and filtered with a mode filter + // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter #endif @@ -1865,9 +1872,15 @@ static void update_altitude() climb_rate = baro_rate; } + // simple smoothing + climb_rate = (climb_rate + old_climb_rate)>>1; + // manage bad data climb_rate = constrain(climb_rate, -300, 300); + // save for filtering + old_climb_rate = climb_rate; + // update the target altitude next_WP.alt = get_new_altitude(); } @@ -1896,14 +1909,18 @@ static void tuning(){ switch(g.radio_tuning){ case CH6_DAMP: - g.rc_6.set_range(0,80); // 0 to 1 + g.rc_6.set_range(0,300); // 0 to 1 g.stablize_d.set(tuning_value); + + //g.rc_6.set_range(0,60); // 0 to 1 + //g.pid_rate_roll.kD(tuning_value); + //g.pid_rate_pitch.kD(tuning_value); break; case CH6_STABILIZE_KP: g.rc_6.set_range(0,8000); // 0 to 8 - g.pi_stabilize_roll.kP(tuning_value); - g.pi_stabilize_pitch.kP(tuning_value); + g.pid_rate_roll.kP(tuning_value); + g.pid_rate_pitch.kP(tuning_value); break; case CH6_STABILIZE_KI: @@ -1915,16 +1932,14 @@ static void tuning(){ case CH6_RATE_KP: g.rc_6.set_range(40,300); // 0 to .3 - g.pi_rate_roll.kP(tuning_value); - g.pi_rate_pitch.kP(tuning_value); - g.pi_acro_roll.kP(tuning_value); - g.pi_acro_pitch.kP(tuning_value); + g.pid_rate_roll.kP(tuning_value); + g.pid_rate_pitch.kP(tuning_value); break; case CH6_RATE_KI: g.rc_6.set_range(0,300); // 0 to .3 - g.pi_rate_roll.kI(tuning_value); - g.pi_rate_pitch.kI(tuning_value); + g.pid_rate_roll.kI(tuning_value); + g.pid_rate_pitch.kI(tuning_value); break; case CH6_YAW_KP: @@ -1934,12 +1949,12 @@ static void tuning(){ case CH6_YAW_RATE_KP: g.rc_6.set_range(0,1000); - g.pi_rate_yaw.kP(tuning_value); + g.pid_rate_yaw.kP(tuning_value); break; case CH6_THROTTLE_KP: g.rc_6.set_range(0,1000); // 0 to 1 - g.pi_throttle.kP(tuning_value); + g.pid_throttle.kP(tuning_value); break; case CH6_TOP_BOTTOM_RATIO: @@ -1966,8 +1981,8 @@ static void tuning(){ case CH6_NAV_P: g.rc_6.set_range(0,6000); - g.pi_nav_lat.kP(tuning_value); - g.pi_nav_lon.kP(tuning_value); + g.pid_nav_lat.kP(tuning_value); + g.pid_nav_lon.kP(tuning_value); break; #if FRAME_CONFIG == HELI_FRAME @@ -1984,20 +1999,20 @@ static void tuning(){ case CH6_OPTFLOW_KP: g.rc_6.set_range(0,5000); // 0 to 5 - g.pi_optflow_roll.kP(tuning_value); - g.pi_optflow_pitch.kP(tuning_value); + g.pid_optflow_roll.kP(tuning_value); + g.pid_optflow_pitch.kP(tuning_value); break; case CH6_OPTFLOW_KI: g.rc_6.set_range(0,10000); // 0 to 10 - g.pi_optflow_roll.kI(tuning_value); - g.pi_optflow_pitch.kI(tuning_value); + g.pid_optflow_roll.kI(tuning_value); + g.pid_optflow_pitch.kI(tuning_value); break; case CH6_OPTFLOW_KD: g.rc_6.set_range(0,200); // 0 to 0.2 - g.pi_optflow_roll.kD(tuning_value); - g.pi_optflow_pitch.kD(tuning_value); + g.pid_optflow_roll.kD(tuning_value); + g.pid_optflow_pitch.kD(tuning_value); break; } } @@ -2075,8 +2090,9 @@ static void update_nav_wp() }else if(wp_control == NO_NAV_MODE){ // clear out our nav so we can do things like land straight down + // or change Loiter position - // We bring in our iterms for wind control, but we don't navigate + // We bring copy over our Iterms for wind control, but we don't navigate nav_lon = g.pi_loiter_lon.get_integrator(); nav_lat = g.pi_loiter_lat.get_integrator(); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 0851d90333..d7c1c75c70 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -3,134 +3,131 @@ static int get_stabilize_roll(int32_t target_angle) { - int32_t error = 0; - int32_t rate = 0; - - static float current_rate = 0; - // angle error - error = wrap_180(target_angle - dcm.roll_sensor); + target_angle = wrap_180(target_angle - dcm.roll_sensor); #if FRAME_CONFIG == HELI_FRAME + // limit the error we're feeding to the PID - error = constrain(error, -4500, 4500); + target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: - rate = g.pi_stabilize_roll.get_pi(error, G_Dt); + target_angle = g.pi_stabilize_roll.get_pi(target_angle, G_Dt); // output control: - rate = constrain(rate, -4500, 4500); - return (int)rate; + return constrain(target_angle, -4500, 4500); #else + // limit the error we're feeding to the PID - error = constrain(error, -2500, 2500); + target_angle = constrain(target_angle, -2500, 2500); // conver to desired Rate: - rate = g.pi_stabilize_roll.get_p(error); + int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle); + int16_t iterm = g.pi_stabilize_roll.get_i(target_angle, G_Dt); - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt); - - // rate control - error = rate - (omega.x * DEGX100); - rate = g.pi_rate_roll.get_pi(error, G_Dt); - - // D term - current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3; - int16_t d_temp = current_rate * g.stablize_d; - rate -= d_temp; - - // output control: - rate = constrain(rate, -2500, 2500); - return (int)rate + iterm; + return get_rate_roll(target_rate) + iterm; #endif } static int get_stabilize_pitch(int32_t target_angle) { - int32_t error = 0; - int32_t rate = 0; - static float current_rate = 0; - // angle error - error = wrap_180(target_angle - dcm.pitch_sensor); + target_angle = wrap_180(target_angle - dcm.pitch_sensor); #if FRAME_CONFIG == HELI_FRAME // limit the error we're feeding to the PID - error = constrain(error, -4500, 4500); + target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: - rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); + target_angle = g.pi_stabilize_pitch.get_pi(target_angle, G_Dt); // output control: - rate = constrain(rate, -4500, 4500); - return (int)rate; + return constrain(target_angle, -4500, 4500); #else - // angle error - error = constrain(error, -2500, 2500); + // limit the error we're feeding to the PID + target_angle = constrain(target_angle, -2500, 2500); // conver to desired Rate: - rate = g.pi_stabilize_pitch.get_p(error); + int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle); + int16_t iterm = g.pi_stabilize_pitch.get_i(target_angle, G_Dt); - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_pitch.get_i(error, G_Dt); - - // rate control - error = rate - (omega.y * DEGX100); - - //error = rate - (omega.y * DEGX100); - rate = g.pi_rate_pitch.get_pi(error, G_Dt); - - // D term - current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3; - int16_t d_temp = current_rate * g.stablize_d; - rate -= d_temp; - - // output control: - rate = constrain(rate, -2500, 2500); - return (int)rate + iterm; + return get_rate_pitch(target_rate) + iterm; #endif } - -#define YAW_ERROR_MAX 2000 static int get_stabilize_yaw(int32_t target_angle) { - int32_t error; - int32_t rate; - // angle error - error = wrap_180(target_angle - dcm.yaw_sensor); + target_angle = wrap_180(target_angle - dcm.yaw_sensor); // limit the error we're feeding to the PID - error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX); + target_angle = constrain(target_angle, -2000, 2000); - // convert to desired Rate: - rate = g.pi_stabilize_yaw.get_p(error); - - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt); + // conver to desired Rate: + int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle); + int16_t iterm = g.pi_stabilize_yaw.get_i(target_angle, G_Dt); #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters - if( !g.heli_ext_gyro_enabled ) { - error = rate - (omega.z * DEGX100); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); + if(!g.heli_ext_gyro_enabled){ + return get_rate_yaw(target_rate) + iterm; + }else{ + return constrain((target_rate + iterm), -4500, 4500); } - // output control: - rate = constrain(rate, -4500, 4500); #else - error = rate - (omega.z * DEGX100); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); + return get_rate_yaw(target_rate) + iterm; +#endif +} + +static int +get_rate_roll(int32_t target_rate) +{ + static int32_t last_rate = 0; + int32_t current_rate = (omega.x * DEGX100); + + // rate control + target_rate = target_rate - current_rate; + target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt); + + // Dampening + target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500); + last_rate = current_rate; // output control: - int16_t yaw_input = 1400 + abs(g.rc_4.control_in); - // smoother Yaw control: - rate = constrain(rate, -yaw_input, yaw_input); -#endif + return constrain(target_rate, -2500, 2500); +} - return (int)rate + iterm; +static int +get_rate_pitch(int32_t target_rate) +{ + static int32_t last_rate = 0; + int32_t current_rate = (omega.y * DEGX100); + + // rate control + target_rate = target_rate - current_rate; + target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt); + + // Dampening + target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500); + last_rate = current_rate; + + // output control: + return constrain(target_rate, -2500, 2500); +} + +static int +get_rate_yaw(int32_t target_rate) +{ + // rate control + target_rate = target_rate - (omega.z * DEGX100); + target_rate = g.pid_rate_yaw.get_pid(target_rate, G_Dt); + + // output control: + int16_t yaw_limit = 1400 + abs(g.rc_4.control_in); + + // smoother Yaw control: + return constrain(target_rate, -yaw_limit, yaw_limit); } #define ALT_ERROR_MAX 400 @@ -138,10 +135,8 @@ static int16_t get_nav_throttle(int32_t z_error) { static int16_t old_output = 0; - //static int16_t rate_d = 0; - - int16_t rate_error; - int16_t output; + int16_t rate_error = 0; + int16_t output = 0; // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); @@ -155,17 +150,10 @@ get_nav_throttle(int32_t z_error) // calculate rate error rate_error = rate_error - climb_rate; - // limit the rate - iterm is not used - output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180); - - // a positive climb rate means we're going up - //rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain - - // slight adjustment to alt hold output - //output -= constrain(rate_d, -25, 25); + // limit the rate + output = constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180); // light filter of output - //output = (old_output * 3 + output) / 4; output = (old_output + output) / 2; // save our output @@ -175,33 +163,6 @@ get_nav_throttle(int32_t z_error) return output + iterm; } -static int -get_rate_roll(int32_t target_rate) -{ - int32_t error = (target_rate * 3.5) - (omega.x * DEGX100); - error = constrain(error, -20000, 20000); - return g.pi_acro_roll.get_pi(error, G_Dt); -} - -static int -get_rate_pitch(int32_t target_rate) -{ - int32_t error = (target_rate * 3.5) - (omega.y * DEGX100); - error = constrain(error, -20000, 20000); - return g.pi_acro_pitch.get_pi(error, G_Dt); -} - -static int -get_rate_yaw(int32_t target_rate) -{ - - int32_t error = (target_rate * 4.5) - (omega.z * DEGX100); - target_rate = g.pi_rate_yaw.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); -} - // Keeps old data out of our calculation / logs static void reset_nav_params(void) { @@ -247,17 +208,15 @@ static void reset_I_all(void) static void reset_rate_I() { - g.pi_rate_roll.reset_I(); - g.pi_rate_pitch.reset_I(); - g.pi_acro_roll.reset_I(); - g.pi_acro_pitch.reset_I(); - g.pi_rate_yaw.reset_I(); + g.pid_rate_roll.reset_I(); + g.pid_rate_pitch.reset_I(); + g.pid_rate_yaw.reset_I(); } static void reset_optflow_I(void) { - g.pi_optflow_roll.reset_I(); - g.pi_optflow_pitch.reset_I(); + g.pid_optflow_roll.reset_I(); + g.pid_optflow_pitch.reset_I(); of_roll = 0; of_pitch = 0; } @@ -272,15 +231,15 @@ static void reset_wind_I(void) static void reset_nav_I(void) { // Rate control for WP navigation - g.pi_nav_lat.reset_I(); - g.pi_nav_lon.reset_I(); + g.pid_nav_lat.reset_I(); + g.pid_nav_lon.reset_I(); } static void reset_throttle_I(void) { // For Altitude Hold g.pi_alt_hold.reset_I(); - g.pi_throttle.reset_I(); + g.pid_throttle.reset_I(); } static void reset_stability_I(void) @@ -325,7 +284,7 @@ static int get_angle_boost(int value) float temp = cos_pitch_x * cos_roll_x; temp = 1.0 - constrain(temp, .5, 1.0); int16_t output = temp * value; - return constrain(output, 0, 100); + return constrain(output, 0, 200); // return (int)(temp * value); } @@ -482,9 +441,9 @@ get_of_roll(int32_t control_roll) // only stop roll if caller isn't modifying roll if( control_roll == 0 && current_loc.alt < 1500) { - new_roll = g.pi_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + //new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ - g.pi_optflow_roll.reset_I(); + g.pid_optflow_roll.reset_I(); tot_x_cm = 0; } // limit amount of change and maximum angle @@ -516,10 +475,10 @@ get_of_pitch(int32_t control_pitch) // only stop roll if caller isn't modifying pitch if( control_pitch == 0 && current_loc.alt < 1500 ) { - new_pitch = g.pi_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + //new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ tot_y_cm = 0; - g.pi_optflow_pitch.reset_I(); + g.pid_optflow_pitch.reset_I(); } // limit amount of change diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 44e74f21bf..355038a9c2 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -824,7 +824,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_ACTION_EMCY_LAND: - //set_mode(LAND); + set_mode(LAND); break; case MAV_ACTION_HALT: @@ -1529,6 +1529,23 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) imu.set_gyro(gyros); */ //imu.set_accel(accels); + + // rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM + Vector3f gyros; + gyros.x = (float)packet.rollspeed / 1000.0; + gyros.y = (float)packet.pitchspeed / 1000.0; + gyros.z = (float)packet.yawspeed / 1000.0; + + imu.set_gyro(gyros); + + // m/s/s + Vector3f accels; + accels.x = (float)packet.roll * gravity / 1000.0; + accels.y = (float)packet.pitch * gravity / 1000.0; + accels.z = (float)packet.yaw * gravity / 1000.0; + + imu.set_accel(accels); + break; } #endif diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 090e2b3501..9b1511c07e 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -590,7 +590,7 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt(climb_rate); // 10 DataFlash.WriteInt(g.rc_3.servo_out); // 11 DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12 - DataFlash.WriteInt(g.pi_throttle.get_integrator()); // 13 + DataFlash.WriteInt(g.pid_throttle.get_integrator()); // 13 DataFlash.WriteByte(END_BYTE); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4c061a6026..0dc92f2d19 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 114; + static const uint16_t k_format_version = 115; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -26,7 +26,6 @@ public: // static const uint16_t k_software_type = 10; // 0 for APM trunk - // // Parameter identities. // // The enumeration defined here is used to ensure that every parameter @@ -168,23 +167,20 @@ public: // 235: PI/D Controllers // k_param_stabilize_d = 234, - k_param_pi_rate_roll = 235, - k_param_pi_rate_pitch, - k_param_pi_rate_yaw, + k_param_pid_rate_roll = 235, + k_param_pid_rate_pitch, + k_param_pid_rate_yaw, k_param_pi_stabilize_roll, k_param_pi_stabilize_pitch, k_param_pi_stabilize_yaw, k_param_pi_loiter_lat, k_param_pi_loiter_lon, - k_param_pi_nav_lat, - k_param_pi_nav_lon, + k_param_pid_nav_lat, + k_param_pid_nav_lon, k_param_pi_alt_hold, - k_param_pi_throttle, - k_param_pi_acro_roll, - k_param_pi_acro_pitch, - k_param_pi_optflow_roll, - k_param_pi_optflow_pitch, // 250 - k_param_loiter_d, + k_param_pid_throttle, + k_param_pid_optflow_roll, + k_param_pid_optflow_pitch, // 250 // 254,255: reserved }; @@ -286,31 +282,24 @@ public: AP_Float camera_pitch_gain; AP_Float camera_roll_gain; AP_Float stablize_d; - AP_Float loiter_d; // PI/D controllers - APM_PI pi_rate_roll; - APM_PI pi_rate_pitch; - APM_PI pi_rate_yaw; + AC_PID pid_rate_roll; + AC_PID pid_rate_pitch; + AC_PID pid_rate_yaw; + AC_PID pid_nav_lat; + AC_PID pid_nav_lon; - APM_PI pi_stabilize_roll; - APM_PI pi_stabilize_pitch; - APM_PI pi_stabilize_yaw; + AC_PID pid_throttle; + AC_PID pid_optflow_roll; + AC_PID pid_optflow_pitch; APM_PI pi_loiter_lat; APM_PI pi_loiter_lon; - - APM_PI pi_nav_lat; - APM_PI pi_nav_lon; - + APM_PI pi_stabilize_roll; + APM_PI pi_stabilize_pitch; + APM_PI pi_stabilize_yaw; APM_PI pi_alt_hold; - APM_PI pi_throttle; - - APM_PI pi_acro_roll; - APM_PI pi_acro_pitch; - - PID pi_optflow_roll; - PID pi_optflow_pitch; uint8_t junk; @@ -369,7 +358,7 @@ public: radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")), - ch7_option (CH7_SAVE_WP, k_param_ch7_option, PSTR("CH7_OPT")), + ch7_option (CH7_OPTION, k_param_ch7_option, PSTR("CH7_OPT")), #if FRAME_CONFIG == HELI_FRAME heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")), @@ -409,34 +398,32 @@ public: //------------------------------------------------------------------------------------------------------------------- camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")), camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")), - stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")), - loiter_d (LOITER_D, k_param_loiter_d, PSTR("LOITER_D")), + + // PID controller group key name initial P initial I initial D initial imax + //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- + pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100), + pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100), + pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100), + + pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), + pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), + + pid_throttle (k_param_pid_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX), + pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_IMAX * 100), + pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_IMAX * 100), + // PI controller group key name initial P initial I initial imax //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- - pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100), - pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100), - pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100), - pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100), pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100), pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100), + pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX), pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), - 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_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_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100), - pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100), - - pi_optflow_roll (k_param_pi_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100), - pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100), 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 d8bd2bbd36..d491201529 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -16,6 +16,7 @@ static void process_nav_command() break; case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint + yaw_mode = YAW_HOLD; do_land(); break; @@ -121,8 +122,6 @@ static void process_now_command() static bool verify_must() { - //Serial.printf("vmust: %d\n", command_nav_ID); - switch(command_nav_queue.id) { case MAV_CMD_NAV_TAKEOFF: @@ -130,7 +129,11 @@ static bool verify_must() break; case MAV_CMD_NAV_LAND: - return verify_land(); + if(g.sonar_enabled == true){ + return verify_land_sonar(); + }else{ + return verify_land_baro(); + } break; case MAV_CMD_NAV_WAYPOINT: @@ -265,13 +268,21 @@ static void do_land() { wp_control = LOITER_MODE; + // just to make sure land_complete = false; + // landing boost lowers the main throttle to mimmick + // the effect of a user's hand + landing_boost = 0; + + // A counter that goes up if our climb rate stalls out. + ground_detector = 0; + // hold at our current location set_next_WP(¤t_loc); - // Set a new target altitude - set_new_altitude(-200); + // Set a new target altitude very low, incase we are landing on a hill! + set_new_altitude(-1000); } static void do_loiter_unlimited() @@ -341,46 +352,71 @@ static bool verify_takeoff() } // called at 10hz -static bool verify_land() +static bool verify_land_sonar() { - static int16_t velocity_land = -1; - static float icount = 0; + static float icount = 1; - // landing detector - if (current_loc.alt > 300){ - velocity_land = 2000; + if(current_loc.alt > 300){ + wp_control = LOITER_MODE; icount = 1; - + ground_detector = 0; }else{ - // a LP filter used to tell if we have landed - // will drive to 0 if we are on the ground - maybe, the baro is noisy - velocity_land = ((velocity_land * 7) + climb_rate ) / 8; + // begin to pull down on the throttle + landing_boost++; } - if ((current_loc.alt - home.alt) < 200){ - // don't bank to hold position - wp_control = NO_NAV_MODE; - // try and come down faster - //landing_boost++; - //landing_boost = min(landing_boost, 15); - float tmp = (1.75 * icount * icount) - (7.2 * icount); - landing_boost = tmp; - landing_boost = constrain(landing_boost, 1, 200); - icount += 0.4; - - //Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount); - - }else{ - landing_boost = 0; - wp_control = LOITER_MODE; + if(current_loc.alt < 200 ){ + wp_control = NO_NAV_MODE; } - //if(/*(current_loc.alt < 100) && */ (velocity_land < 20)){ - if((landing_boost > 150) || (velocity_land < 20)){ - icount = 1; - land_complete = true; - landing_boost = 0; - return true; + if(current_loc.alt < 150 ){ + //rapid throttle reduction + int16_t lb = (1.75 * icount * icount) - (7.2 * icount); + icount++; + lb = constrain(lb, 0, 180); + landing_boost += lb; + //Serial.printf("%0.0f, %d, %d, %d\n", icount, current_loc.alt, landing_boost, lb); + + if(current_loc.alt < 40 || abs(climb_rate) < 20) { + if(ground_detector++ > 20) { + land_complete = true; + ground_detector = 0; + icount = 1; + // init disarm motors + init_disarm_motors(); + return true; + } + } + } + return false; +} + +static bool verify_land_baro() +{ + if(current_loc.alt > 300){ + wp_control = LOITER_MODE; + ground_detector = 0; + }else{ + // begin to pull down on the throttle + landing_boost++; + landing_boost = min(landing_boost, 40); + } + if(current_loc.alt < 200 ){ + wp_control = NO_NAV_MODE; + } + + if(current_loc.alt < 150 ){ + if(abs(climb_rate) < 20) { + if(ground_detector++ > 30) { + //landing_boost = 100; + //Serial.print("land_complete\n"); + land_complete = true; + ground_detector = 0; + // init disarm motors + init_disarm_motors(); + return true; + } + } } return false; } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 929c5c35b6..4c3b94eda7 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -62,14 +62,7 @@ static void update_commands() g.command_index = command_nav_index = 255; // if we are on the ground, enter stabilize, else Land if (land_complete == true){ - // So what state does this leave us in? - // We are still in the same mode as what landed us, - // so maybe we try to continue to descend just in case we are still in the air - // This will also drive down the Iterm to -300 - - // We can't disarm the motors easily. We could very well be wrong - // - //init_disarm_motors(); + // we will disarm the motors after landing. } else { set_mode(LAND); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 4d07062990..2fc2e3e66e 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -525,114 +525,98 @@ #ifdef MOTORS_JD880 # define STABILIZE_ROLL_P 3.6 # define STABILIZE_ROLL_I 0.0 -# define STABILIZE_ROLL_IMAX 40.0 // degrees +# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_PITCH_P 3.6 # define STABILIZE_PITCH_I 0.0 -# define STABILIZE_PITCH_IMAX 40.0 // degrees +# define STABILIZE_PITCH_IMAX 40.0 // degrees #endif #ifdef MOTORS_JD850 # define STABILIZE_ROLL_P 4.0 # define STABILIZE_ROLL_I 0.0 -# define STABILIZE_ROLL_IMAX 40.0 // degrees +# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_PITCH_P 4.0 # define STABILIZE_PITCH_I 0.0 -# define STABILIZE_PITCH_IMAX 40.0 // degrees +# define STABILIZE_PITCH_IMAX 40.0 // degrees #endif #ifndef STABILIZE_D -# define STABILIZE_D .03 +# define STABILIZE_D .12 #endif // Jasons default values that are good for smaller payload motors. #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4.6 +# define STABILIZE_ROLL_P 4.5 #endif #ifndef STABILIZE_ROLL_I -# define STABILIZE_ROLL_I 0.1 +# define STABILIZE_ROLL_I 0.0 #endif #ifndef STABILIZE_ROLL_IMAX # define STABILIZE_ROLL_IMAX 40 // degrees #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 4.6 +# define STABILIZE_PITCH_P 4.5 #endif #ifndef STABILIZE_PITCH_I -# define STABILIZE_PITCH_I 0.1 +# define STABILIZE_PITCH_I 0.0 #endif #ifndef STABILIZE_PITCH_IMAX # define STABILIZE_PITCH_IMAX 40 // degrees #endif -////////////////////////////////////////////////////////////////////////////// -// Acro Rate Control -// -#ifndef ACRO_ROLL_P -# define ACRO_ROLL_P 0.155 +#ifndef STABILIZE_YAW_P +# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif -#ifndef ACRO_ROLL_I -# define ACRO_ROLL_I 0.0 +#ifndef STABILIZE_YAW_I +# define STABILIZE_YAW_I 0.01 #endif -#ifndef ACRO_ROLL_IMAX -# define ACRO_ROLL_IMAX 15 // degrees +#ifndef STABILIZE_YAW_IMAX +# define STABILIZE_YAW_IMAX 8 // degrees * 100 #endif -#ifndef ACRO_PITCH_P -# define ACRO_PITCH_P 0.155 -#endif -#ifndef ACRO_PITCH_I -# define ACRO_PITCH_I 0 //0.18 -#endif -#ifndef ACRO_PITCH_IMAX -# define ACRO_PITCH_IMAX 15 // degrees -#endif ////////////////////////////////////////////////////////////////////////////// // Stabilize Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P 0.155 +# define RATE_ROLL_P 0.14 #endif #ifndef RATE_ROLL_I -# define RATE_ROLL_I 0.0 +# define RATE_ROLL_I 0.0 +#endif +#ifndef RATE_ROLL_D +# define RATE_ROLL_D 0 #endif #ifndef RATE_ROLL_IMAX # define RATE_ROLL_IMAX 15 // degrees #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.155 +# define RATE_PITCH_P 0.14 #endif #ifndef RATE_PITCH_I -# define RATE_PITCH_I 0 //0.18 +# define RATE_PITCH_I 0 // 0.18 +#endif +#ifndef RATE_PITCH_D +# define RATE_PITCH_D 0 // 0.002 #endif #ifndef RATE_PITCH_IMAX # define RATE_PITCH_IMAX 15 // degrees #endif -////////////////////////////////////////////////////////////////////////////// -// YAW Control -// -#ifndef STABILIZE_YAW_P -# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy -#endif -#ifndef STABILIZE_YAW_I -# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance -#endif -#ifndef STABILIZE_YAW_IMAX -# define STABILIZE_YAW_IMAX 8 // degrees * 100 -#endif - #ifndef RATE_YAW_P -# define RATE_YAW_P .13 // used to control response in turning +# define RATE_YAW_P .13 #endif #ifndef RATE_YAW_I -# define RATE_YAW_I 0.0 +# define RATE_YAW_I 0.0 +#endif +#ifndef RATE_YAW_D +# define RATE_YAW_D .002 #endif #ifndef RATE_YAW_IMAX -# define RATE_YAW_IMAX 50 +# define RATE_YAW_IMAX 50 // degrees #endif @@ -640,37 +624,37 @@ // Loiter control gains // #ifndef LOITER_P -# define LOITER_P 1.5 // was .25 in previous +# define LOITER_P .4 // was .25 in previous #endif #ifndef LOITER_I -# define LOITER_I 0.09 // Wind control +# define LOITER_I 0.2 // Wind control #endif #ifndef LOITER_IMAX -# define LOITER_IMAX 30 // degrees° -#endif -#ifndef LOITER_D -# define LOITER_D 2.2 // rate control +# define LOITER_IMAX 30 // degrees #endif ////////////////////////////////////////////////////////////////////////////// // WP Navigation control gains // #ifndef NAV_P -# define NAV_P 3.0 // 3 was causing rapid oscillations in Loiter +# define NAV_P 2.3 // 3 was causing rapid oscillations in Loiter #endif #ifndef NAV_I # define NAV_I 0 // #endif +#ifndef NAV_D +# define NAV_D 0.015 // +#endif #ifndef NAV_IMAX # define NAV_IMAX 30 // degrees #endif #ifndef WAYPOINT_SPEED_MAX -# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph #endif #ifndef WAYPOINT_SPEED_MIN -# define WAYPOINT_SPEED_MIN 100 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MIN 100 // 1m/s #endif @@ -678,28 +662,31 @@ // Throttle control gains // #ifndef THROTTLE_CRUISE -# define THROTTLE_CRUISE 350 // +# define THROTTLE_CRUISE 350 // #endif -#ifndef THR_HOLD_P -# define THR_HOLD_P 0.4 // +#ifndef ALT_HOLD_P +# define ALT_HOLD_P 0.4 // #endif -#ifndef THR_HOLD_I -# define THR_HOLD_I 0.01 // with 4m error, 12.5s windup +#ifndef ALT_HOLD_I +# define ALT_HOLD_I 0.02 #endif -#ifndef THR_HOLD_IMAX -# define THR_HOLD_IMAX 300 +#ifndef ALT_HOLD_IMAX +# define ALT_HOLD_IMAX 300 #endif // RATE control #ifndef THROTTLE_P -# define THROTTLE_P 0.5 // +# define THROTTLE_P 0.5 // #endif #ifndef THROTTLE_I -# define THROTTLE_I 0.0 // +# define THROTTLE_I 0.0 // +#endif +#ifndef THROTTLE_D +# define THROTTLE_D 0.02 // #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 8911fb0962..87965160eb 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -114,6 +114,7 @@ static void read_trim_switch() // reset the mission CH7_wp_index = 0; g.command_total.set_and_save(1); + set_mode(RTL); return; } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a8795de1eb..a0eb0e00d7 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -7,13 +7,11 @@ static byte navigate() { // waypoint distance from plane in meters // --------------------------------------- - wp_distance = get_distance(¤t_loc, &next_WP); - home_distance = get_distance(¤t_loc, &home); + wp_distance = get_distance(¤t_loc, &next_WP); + home_distance = get_distance(¤t_loc, &home); if (wp_distance < 0){ - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); - //Serial.println(wp_distance,DEC); - //print_current_waypoints(); + // something went very wrong return 0; } @@ -61,9 +59,8 @@ static void calc_XY_velocity(){ int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp; // slight averaging filter - x_GPS_speed = (x_GPS_speed + x_estimate) >> 1; - y_GPS_speed = (y_GPS_speed + y_estimate) >> 1; - //*/ + x_actual_speed = (x_actual_speed + x_estimate) >> 1; + y_actual_speed = (y_actual_speed + y_estimate) >> 1; /* // Ryan Beall's forward estimator: @@ -99,67 +96,7 @@ static void calc_location_error(struct Location *next_loc) lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North } -/* -//static void calc_loiter3(int x_error, int y_error) -{ - static int32_t gps_lat_I = 0; - static int32_t gps_lon_I = 0; - - // If close to goal <1m reset the I term - if (abs(x_error) < 50) - gps_lon_I = 0; - if (abs(y_error) < 50) - gps_lat_I = 0; - - gps_lon_I += x_error; - gps_lat_I += y_error; - - gps_lon_I = constrain(gps_lon_I,-3000,3000); - gps_lat_I = constrain(gps_lat_I,-3000,3000); - - int16_t lon_P = 1.2 * (float)x_error; - int16_t lon_I = 0.1 * (float)gps_lon_I; //.1 - int16_t lon_D = 3 * x_GPS_speed ; // this controls the small bumps - - int16_t lat_P = 1.2 * (float)y_error; - int16_t lat_I = 0.1 * (float)gps_lat_I; - int16_t lat_D = 3 * y_GPS_speed ; - - //limit of terms - lon_I = constrain(lon_I,-3000,3000); - lat_I = constrain(lat_I,-3000,3000); - lon_D = constrain(lon_D,-500,500); //this controls the long distance dampimg - lat_D = constrain(lat_D,-500,500); //this controls the long distance dampimg - - nav_lon = lon_P + lon_I - lon_D; - nav_lat = lat_P + lat_I - lat_D; - - Serial.printf("%d, %d, %d, %d, %d, %d\n", - lon_P, lat_P, - lon_I, lat_I, - lon_D, lat_D); - -} -*/ - #define NAV_ERR_MAX 800 -static void calc_loiter1(int x_error, int y_error) -{ - int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav); - int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps - - int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav); - int16_t lat_D = g.loiter_d * y_actual_speed ; - - //limit of terms - lon_D = constrain(lon_D,-500,500); - lat_D = constrain(lat_D,-500,500); - - nav_lon = constrain(lon_PI - lon_D, -2500, 2500); - nav_lat = constrain(lat_PI - lat_D, -2500, 2500); -} - - static void calc_loiter(int x_error, int y_error) { // East/West @@ -167,22 +104,20 @@ static void calc_loiter(int x_error, int y_error) int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); x_rate_error = x_target_speed - x_actual_speed; - nav_lon_p = x_rate_error * g.loiter_d; - - nav_lon_p = constrain(nav_lon_p, -1200, 1200); - nav_lon = nav_lon_p + x_iterm; - nav_lon = constrain(nav_lon, -2500, 2500); // North/South y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error); int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); y_rate_error = y_target_speed - y_actual_speed; - nav_lat_p = y_rate_error * g.loiter_d; - nav_lat_p = constrain(nav_lat_p, -1200, 1200); - nav_lat = nav_lat_p + y_iterm; - nav_lat = constrain(nav_lat, -2500, 2500); + calc_nav_lon(x_rate_error); + calc_nav_lat(y_rate_error); + + nav_lat = nav_lat + y_iterm; + nav_lon = nav_lon + x_iterm; + + /* int8_t ttt = 1.0/dTnav; @@ -204,7 +139,7 @@ static void calc_loiter(int x_error, int y_error) //*/ /* - int16_t t1 = g.pi_nav_lon.get_integrator(); // X + int16_t t1 = g.pid_nav_lon.get_integrator(); // X Serial.printf("%d, %1.4f, %d, %d, %d, %d, %d, %d, %d, %d\n", wp_distance, //1 dTnav, //2 @@ -219,33 +154,91 @@ static void calc_loiter(int x_error, int y_error) //*/ } +static void calc_nav_rate(int max_speed) +{ + // push us towards the original track + update_crosstrack(); + + // nav_bearing includes crosstrack + float temp = (9000l - nav_bearing) * RADX100; + + x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 + x_rate_error = constrain(x_rate_error, -1000, 1000); + int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); + + y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum + int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); + + calc_nav_lon(x_rate_error); + calc_nav_lat(y_rate_error); + + nav_lon = nav_lon + x_iterm; + nav_lat = nav_lat + y_iterm; + + /* + Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat, + x_iterm, + y_iterm, + crosstrack_error); + //*/ + + // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() + + /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat);*/ +} + + +static void calc_nav_lon(int rate) +{ + nav_lon = g.pid_nav_lon.get_pid(rate, dTnav); + nav_lon = get_corrected_angle(rate, nav_lon); + nav_lon = constrain(nav_lon, -3000, 3000); +} + +static void calc_nav_lat(int rate) +{ + nav_lat = g.pid_nav_lon.get_pid(rate, dTnav); + nav_lat = get_corrected_angle(rate, nav_lat); + nav_lat = constrain(nav_lat, -3000, 3000); +} + +static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out) +{ + int16_t tt = desired_rate; + // scale down the desired rate and square it + desired_rate = desired_rate / 20; + desired_rate = desired_rate * desired_rate; + int16_t tmp = 0; + + if (tt > 0){ + tmp = rate_out + (rate_out - desired_rate); + tmp = max(tmp, rate_out); + }else if (tt < 0){ + tmp = rate_out + (rate_out + desired_rate); + tmp = min(tmp, rate_out); + } + //Serial.printf("rate:%d, norm:%d, out:%d \n", tt, rate_out, tmp); + return tmp; +} + //wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2 -#define ERR_GAIN .01 -// called at 50hz -static void estimate_velocity() -{ - // we need to extimate velocity when below GPS threshold of 1.5m/s - //if(g_gps->ground_speed < 120){ - // some smoothing to prevent bumpy rides - x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16; - y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16; - - // integration of nav_p angle - //x_actual_speed += (nav_lon_p >>2); - //y_actual_speed += (nav_lat_p >>2); - - // this is just what worked best in SIM - //x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4; - //y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4; - - //}else{ - // less smoothing needed since the GPS already filters - // x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4; - // y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4; - //} -} // this calculation rotates our World frame of reference to the copter's frame of reference // We use the DCM's matrix to precalculate these trig values at 50hz @@ -291,55 +284,6 @@ static int16_t calc_desired_speed(int16_t max_speed) return max_speed; } -static void calc_nav_rate(int max_speed) -{ - // push us towards the original track - update_crosstrack(); - - // nav_bearing includes crosstrack - float temp = (9000l - nav_bearing) * RADX100; - - x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 - x_rate_error = constrain(x_rate_error, -1000, 1000); - int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); - nav_lon_p = g.pi_nav_lon.get_p(x_rate_error); - nav_lon = nav_lon_p + x_iterm; - nav_lon = constrain(nav_lon, -3000, 3000); - - y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum - int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); - nav_lat_p = g.pi_nav_lat.get_p(y_rate_error); - nav_lat = nav_lat_p + y_iterm; - nav_lat = constrain(nav_lat, -3000, 3000); - - /* - Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n", - max_speed, - x_actual_speed, - y_actual_speed, - x_rate_error, - y_rate_error, - nav_lon, - nav_lat, - x_iterm, - y_iterm, - crosstrack_error); - //*/ - - - // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() - - /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", - max_speed, - x_actual_speed, - y_actual_speed, - x_rate_error, - y_rate_error, - nav_lon, - nav_lat);*/ -} - static void update_crosstrack(void) { @@ -433,7 +377,7 @@ static int32_t get_new_altitude() } int32_t diff = abs(next_WP.alt - target_altitude); - int8_t _scale = 3; + int8_t _scale = 4; if (next_WP.alt < target_altitude){ // we are below the target alt @@ -444,12 +388,12 @@ static int32_t get_new_altitude() } }else { // we are above the target, going down - if(diff < 600){ - _scale = 4; - } - if(diff < 300){ + if(diff < 400){ _scale = 5; } + if(diff < 100){ + _scale = 6; + } } int32_t change = (millis() - alt_change_timer) >> _scale; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index d4ad437754..9cf32e522a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -354,8 +354,8 @@ static void init_ardupilot() Log_Write_Startup(); Log_Write_Data(10, g.pi_stabilize_roll.kP()); Log_Write_Data(11, g.pi_stabilize_pitch.kP()); - Log_Write_Data(12, g.pi_rate_roll.kP()); - Log_Write_Data(13, g.pi_rate_pitch.kP()); + Log_Write_Data(12, g.pid_rate_roll.kP()); + Log_Write_Data(13, g.pid_rate_pitch.kP()); #endif SendDebug("\nReady to FLY ");