diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4361e54e86..cf370df6c1 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1200,14 +1200,6 @@ static void fifty_hz_loop() // ------------------------- update_throttle_mode(); - // Read Sonar - // ---------- -# if CONFIG_SONAR == ENABLED - if(g.sonar_enabled) { - sonar_alt = sonar.read(); - } -#endif - #if TOY_EDF == ENABLED edf_toy(); #endif @@ -1436,7 +1428,6 @@ static void update_GPS(void) } #if HIL_MODE == HIL_MODE_ATTITUDE // only execute in HIL mode - //update_altitude(); ap_system.alt_sensor_flag = true; #endif } @@ -2015,67 +2006,48 @@ 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; + int32_t old_baro_alt = baro_alt; + int16_t old_sonar_alt = sonar_alt; #if HIL_MODE == HIL_MODE_ATTITUDE // we are in the SIM, fake out the baro and Sonar int16_t fake_relative_alt = g_gps->altitude - gps_base_alt; baro_alt = fake_relative_alt; - sonar_alt = fake_relative_alt; - baro_rate = (baro_alt - old_baro_alt) * 5; // 5hz - old_baro_alt = baro_alt; - -#else - // This is real life - - // read in Actual Baro Altitude - baro_alt = read_barometer(); - - // calc the vertical accel rate - - // 2.6 way - int16_t temp = (baro_alt - old_baro_alt) * 10; - baro_rate = (temp + baro_rate) >> 1; - baro_rate = constrain(baro_rate, -500, 500); - old_baro_alt = baro_alt; - - // Using Tridge's new clamb rate calc - /* - int16_t temp = barometer.get_climb_rate() * 100; - baro_rate = (temp + baro_rate) >> 1; - baro_rate = constrain(baro_rate, -300, 300); - */ - - // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter -#endif - if(g.sonar_enabled) { - // filter out offset - float scale; - - // calc rate of change for Sonar -#if HIL_MODE == HIL_MODE_ATTITUDE - // we are in the SIM, fake outthe Sonar rate - sonar_rate = baro_rate; + sonar_alt = fake_relative_alt; + sonar_rate = baro_rate; + } + current_loc.alt = baro_alt; + climb_rate_actual = baro_rate; #else - // This is real life - // calc the vertical accel rate - // positive = going up. - sonar_rate = (sonar_alt - old_sonar_alt) * 10; - sonar_rate = constrain(sonar_rate, -150, 150); - old_sonar_alt = sonar_alt; -#endif + // read in actual baro altitude + baro_alt = read_barometer(); + // calc baro based vertical velocity + int16_t temp = (baro_alt - old_baro_alt) * 10; + baro_rate = (temp + baro_rate) >> 1; + baro_rate = constrain(baro_rate, -500, 500); + + // read in sonar altitude and calculate sonar rate + if(g.sonar_enabled) { + sonar_alt = read_sonar(); + sonar_rate = (sonar_alt - old_sonar_alt) * 10; + sonar_rate = constrain(sonar_rate, -150, 150); + } + + // Note: with inertial nav, alt and rate are pulled from the inav lib at 50hz in update_altitude_est function + // so none of the below is required +# if INERTIAL_NAV_Z != ENABLED + // if no sonar set current alt to baro alt + if(!g.sonar_enabled) { + // NO Sonar case + current_loc.alt = baro_alt; + climb_rate_actual = baro_rate; + }else{ + // Blend barometer and sonar data together + float scale; if(baro_alt < 800) { -#if SONAR_TILT_CORRECTION == 1 - // correct alt for angle of the sonar - float temp = cos_pitch_x * cos_roll_x; - temp = max(temp, 0.707); - sonar_alt = (float)sonar_alt * temp; -#endif - scale = (float)(sonar_alt - 400) / 200.0; scale = constrain(scale, 0.0, 1.0); // solve for a blended altitude @@ -2088,25 +2060,35 @@ static void update_altitude() // we must be higher than sonar (>800), don't get tricked by bad sonar reads current_loc.alt = baro_alt; // dont blend, go straight baro - climb_rate_actual = baro_rate; } - - }else{ - // NO Sonar case - current_loc.alt = baro_alt; - climb_rate_actual = baro_rate; } + // climb_rate_error is used to spread the change in climb rate across the next 5 samples + climb_rate_error = (climb_rate_actual - climb_rate) / 5; +# endif // INERTIAL_NAV_Z != ENABLED +#endif // HIL_MODE == HIL_MODE_ATTITUDE // update the target altitude verify_altitude(); - - // calc error - climb_rate_error = (climb_rate_actual - climb_rate) / 5; } static void update_altitude_est() { +#if INERTIAL_NAV_Z == ENABLED + // with inertial nav we can update the altitude and climb rate at 50hz + current_loc.alt = inertial_nav.get_altitude(); + climb_rate = inertial_nav.get_velocity_z(); + + // update baro and sonar alt and climb rate just for logging purposes + // To-Do: remove alt_sensor_flag and move update_altitude to be called from 10hz loop + if(ap_system.alt_sensor_flag) { + ap_system.alt_sensor_flag = false; + update_altitude(); + if(g.log_bitmask & MASK_LOG_CTUN && motors.armed()) { + Log_Write_Control_Tuning(); + } + } +#else if(ap_system.alt_sensor_flag) { update_altitude(); ap_system.alt_sensor_flag = false; @@ -2114,12 +2096,12 @@ static void update_altitude_est() if(g.log_bitmask & MASK_LOG_CTUN && motors.armed()) { Log_Write_Control_Tuning(); } - }else{ // simple dithering of climb rate climb_rate += climb_rate_error; current_loc.alt += (climb_rate / 50); } +#endif } static void tuning(){ diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 233c649fe4..769591e12e 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -941,11 +941,7 @@ get_throttle_rate(int16_t z_target_speed) int16_t output; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors // calculate rate error and filter with cut off frequency of 2 Hz -#if INERTIAL_NAV_Z == ENABLED - z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - inertial_nav.get_velocity_z()) - z_rate_error); -#else z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - climb_rate) - z_rate_error); -#endif // separately calculate p, i, d values for logging p = g.pid_throttle.get_p(z_rate_error); @@ -1026,11 +1022,7 @@ get_throttle_althold(int32_t target_alt, int16_t max_climb_rate) int32_t linear_distance; // the distace we swap between linear and sqrt. // calculate altitude error -#if INERTIAL_NAV_Z == ENABLED - altitude_error = target_alt - inertial_nav.get_altitude(); -#else altitude_error = target_alt - current_loc.alt; -#endif linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); @@ -1063,11 +1055,7 @@ get_throttle_land() get_throttle_rate_stabilized(-abs(g.land_speed)); // detect whether we have landed by watching for minimum throttle and now movement -#if INERTIAL_NAV_Z == ENABLED - if (abs(inertial_nav.get_velocity_z()) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { -#else if (abs(climb_rate) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { -#endif if( land_detector < LAND_DETECTOR_TRIGGER ) { land_detector++; }else{ diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 0176162fd5..9ebd23689a 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -604,7 +604,7 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt(next_WP.alt); // 4 DataFlash.WriteInt(nav_throttle); // 5 DataFlash.WriteInt(angle_boost); // 6 - DataFlash.WriteInt(climb_rate_actual); // 7 + DataFlash.WriteInt(climb_rate); // 7 DataFlash.WriteInt(g.rc_3.servo_out); // 8 DataFlash.WriteInt(desired_climb_rate); // 9 @@ -798,7 +798,7 @@ static void Log_Write_INAV(float delta_t) DataFlash.WriteInt((int16_t)baro_alt); // 1 barometer altitude DataFlash.WriteInt((int16_t)inertial_nav.get_altitude()); // 2 accel + baro filtered altitude - DataFlash.WriteInt((int16_t)climb_rate_actual); // 3 barometer based climb rate + DataFlash.WriteInt((int16_t)baro_rate); // 3 barometer based climb rate DataFlash.WriteInt((int16_t)inertial_nav.get_velocity_z()); // 4 accel + baro based climb rate DataFlash.WriteLong(get_int(accel_corr.x)); // 5 accel correction x-axis DataFlash.WriteLong(get_int(accel_corr.y)); // 6 accel correction y-axis diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 22e976490c..eb2377dd04 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -31,6 +31,25 @@ static int32_t read_barometer(void) return baro_filter.apply(barometer.get_altitude() * 100.0); } +// return sonar altitude in centimeters +static int16_t read_sonar(void) +{ +#if CONFIG_SONAR == ENABLED + int16_t temp_alt = sonar.read(); + + #if SONAR_TILT_CORRECTION == 1 + // correct alt for angle of the sonar + float temp = cos_pitch_x * cos_roll_x; + temp = max(temp, 0.707); + temp_alt = (float)temp_alt * temp; + #endif + + return temp_alt; +#else + return 0; +#endif +} + #endif // HIL_MODE != HIL_MODE_ATTITUDE