Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: use inertial nav for current altitude and climb rate

Browse Source
This commit is contained in:
rmackay9 2012-12-21 14:03:47 +09:00
parent d5b4d43625
commit c7a0a64eae
4 changed files with 73 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
ArduCopter/ArduCopter.pde
View File

@ -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(){

12
ArduCopter/Attitude.pde
View File

@ -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{

4
ArduCopter/Log.pde
View File

@ -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

19
ArduCopter/sensors.pde
View File

@ -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