Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: remove baro-only alt hold 

Saves 8 bytes of memory and more importantly simplifies the alt hold
calculations
This commit is contained in:
Randy Mackay 2013-02-02 12:00:09 +09:00 committed by rmackay9
parent 37d3e1d7e4
commit 67c69a19ee
9 changed files with 35 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/APM_Config.h
View File

@ -48,8 +48,6 @@
#define NAV_LOITER_ACTIVE NAV_LOITER_INAV #define NAV_LOITER_ACTIVE NAV_LOITER_INAV
#define RTL_YAW YAW_HOLD #define RTL_YAW YAW_HOLD
#define INERTIAL_NAV_Z ENABLED
//#define MOTORS_JD880 //#define MOTORS_JD880
//#define MOTORS_JD850 //#define MOTORS_JD850

110
ArduCopter/ArduCopter.pde
View File

@ -648,21 +648,13 @@ static float current_total1;
static float controller_desired_alt; static float controller_desired_alt;
// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP // The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP
static int32_t altitude_error; static int32_t altitude_error;
// The cm/s we are moving up or down based on sensor data - Positive = UP
static int16_t climb_rate_actual;
// Used to dither our climb_rate over 50hz
static int16_t climb_rate_error;
// The cm/s we are moving up or down based on filtered data - Positive = UP // The cm/s we are moving up or down based on filtered data - Positive = UP
static int16_t climb_rate; static int16_t climb_rate;
// The altitude as reported by Sonar in cm – Values are 20 to 700 generally. // The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
static int16_t sonar_alt; static int16_t sonar_alt;
static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar
// 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 // The altitude as reported by Baro in cm – Values can be quite high
static int32_t baro_alt; static int32_t baro_alt;
// The climb_rate as reported by Baro in cm/s
static int16_t baro_rate;
static int16_t saved_toy_throttle; static int16_t saved_toy_throttle;
@ -827,9 +819,7 @@ static float G_Dt = 0.02;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Inertial Navigation // Inertial Navigation
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps); AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps);
#endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Performance monitoring // Performance monitoring
@ -931,7 +921,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ update_GPS, 2, 900 }, { update_GPS, 2, 900 },
{ update_navigation, 2, 500 }, { update_navigation, 2, 500 },
{ medium_loop, 2, 700 }, { medium_loop, 2, 700 },
{ update_altitude_est, 2, 1000 }, { update_altitude, 5, 1000 },
{ fifty_hz_loop, 2, 950 }, { fifty_hz_loop, 2, 950 },
{ run_nav_updates, 2, 500 }, { run_nav_updates, 2, 500 },
{ slow_loop, 10, 500 }, { slow_loop, 10, 500 },
@ -1225,6 +1215,9 @@ static void medium_loop()
// --------------------------- // ---------------------------
static void fifty_hz_loop() static void fifty_hz_loop()
{ {
// get altitude and climb rate from inertial lib
read_inertial_altitude();
// Update the throttle ouput // Update the throttle ouput
// ------------------------- // -------------------------
update_throttle_mode(); update_throttle_mode();
@ -2078,108 +2071,29 @@ static void update_trig(void){
// 270 = cos_yaw: -1.00, sin_yaw: 0.00, // 270 = cos_yaw: -1.00, sin_yaw: 0.00,
} }
// updated at 10hz // read baro and sonar altitude at 10hz
static void update_altitude() static void update_altitude()
{ {
int32_t old_baro_alt = baro_alt;
int16_t old_sonar_alt = sonar_alt;
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
// we are in the SIM, fake out the baro and Sonar // we are in the SIM, fake out the baro and Sonar
int16_t fake_relative_alt = g_gps->altitude - gps_base_alt; baro_alt = g_gps->altitude - gps_base_alt;
baro_alt = fake_relative_alt;
baro_rate = (baro_alt - old_baro_alt) * 5; // 5hz
if(g.sonar_enabled) { if(g.sonar_enabled) {
sonar_alt = fake_relative_alt; sonar_alt = baro_alt;
sonar_rate = baro_rate;
} }
current_loc.alt = baro_alt;
climb_rate_actual = baro_rate;
#else #else
// read in actual baro altitude // read in baro altitude
baro_alt = read_barometer(); baro_alt = read_barometer();
// calc baro based vertical velocity // read in sonar altitude
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
sonar_alt = read_sonar(); sonar_alt = read_sonar();
// start calculating the sonar_rate as soon as valid sonar readings start coming in so that we are ready when the sonar_alt_health becomes 3
// Note: post 2.9.1 release we will remove the sonar_rate variable completely
if(sonar_alt_health > 1) {
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) {
scale = (float)(sonar_alt - 400) / 200.0;
scale = constrain(scale, 0.0, 1.0);
// solve for a blended altitude
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale);
// solve for a blended climb_rate
climb_rate_actual = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
}else{
// 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;
}
}
// 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 #endif // HIL_MODE == HIL_MODE_ATTITUDE
// update the target altitude // write altitude info to dataflash logs
verify_altitude();
}
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()) { if ((g.log_bitmask & MASK_LOG_CTUN) && motors.armed()) {
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
} }
} }
#else
if(ap_system.alt_sensor_flag) {
update_altitude();
ap_system.alt_sensor_flag = false;
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(){ static void tuning(){
tuning_value = (float)g.rc_6.control_in / 1000.0f; tuning_value = (float)g.rc_6.control_in / 1000.0f;
@ -2331,9 +2245,7 @@ static void tuning(){
#if INERTIAL_NAV_XY == ENABLED #if INERTIAL_NAV_XY == ENABLED
inertial_nav.set_time_constant_xy(tuning_value); inertial_nav.set_time_constant_xy(tuning_value);
#endif #endif
#if INERTIAL_NAV_Z == ENABLED
inertial_nav.set_time_constant_z(tuning_value); inertial_nav.set_time_constant_z(tuning_value);
#endif
break; break;
case CH6_THR_ACCEL_KP: case CH6_THR_ACCEL_KP:

31
ArduCopter/Log.pde
View File

@ -772,7 +772,6 @@ struct log_INAV {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
int16_t baro_alt; int16_t baro_alt;
int16_t inav_alt; int16_t inav_alt;
int16_t baro_climb_rate;
int16_t inav_climb_rate; int16_t inav_climb_rate;
float accel_corr_x; float accel_corr_x;
float accel_corr_y; float accel_corr_y;
@ -789,28 +788,25 @@ struct log_INAV {
// Write an INAV packet. Total length : 52 Bytes // Write an INAV packet. Total length : 52 Bytes
static void Log_Write_INAV() static void Log_Write_INAV()
{ {
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
Vector3f accel_corr = inertial_nav.accel_correction_ef; Vector3f accel_corr = inertial_nav.accel_correction_ef;
struct log_INAV pkt = { struct log_INAV pkt = {
LOG_PACKET_HEADER_INIT(LOG_INAV_MSG), LOG_PACKET_HEADER_INIT(LOG_INAV_MSG),
baro_alt : (int16_t)baro_alt, // 1 barometer altitude baro_alt : (int16_t)baro_alt, // 1 barometer altitude
inav_alt : (int16_t)inertial_nav.get_altitude(), // 2 accel + baro filtered altitude inav_alt : (int16_t)inertial_nav.get_altitude(), // 2 accel + baro filtered altitude
baro_climb_rate : baro_rate, // 3 barometer based climb rate inav_climb_rate : (int16_t)inertial_nav.get_velocity_z(), // 3 accel + baro based climb rate
inav_climb_rate : (int16_t)inertial_nav.get_velocity_z(), // 4 accel + baro based climb rate accel_corr_x : accel_corr.x, // 4 accel correction x-axis
accel_corr_x : accel_corr.x, // 5 accel correction x-axis accel_corr_y : accel_corr.y, // 5 accel correction y-axis
accel_corr_y : accel_corr.y, // 6 accel correction y-axis accel_corr_z : accel_corr.z, // 6 accel correction z-axis
accel_corr_z : accel_corr.z, // 7 accel correction z-axis accel_corr_ef_z : inertial_nav.accel_correction_ef.z, // 7 accel correction earth frame
accel_corr_ef_z : inertial_nav.accel_correction_ef.z, // 8 accel correction earth frame gps_lat_from_home : g_gps->latitude-home.lat, // 8 lat from home
gps_lat_from_home : g_gps->latitude-home.lat, // 9 lat from home gps_lon_from_home : g_gps->longitude-home.lng, // 9 lon from home
gps_lon_from_home : g_gps->longitude-home.lng, // 10 lon from home inav_lat_from_home : inertial_nav.get_latitude_diff(), // 10 accel based lat from home
inav_lat_from_home : inertial_nav.get_latitude_diff(), // 11 accel based lat from home inav_lon_from_home : inertial_nav.get_longitude_diff(), // 11 accel based lon from home
inav_lon_from_home : inertial_nav.get_longitude_diff(), // 12 accel based lon from home inav_lat_speed : inertial_nav.get_latitude_velocity(), // 12 accel based lat velocity
inav_lat_speed : inertial_nav.get_latitude_velocity(), // 13 accel based lat velocity inav_lon_speed : inertial_nav.get_longitude_velocity() // 13 accel based lon velocity
inav_lon_speed : inertial_nav.get_longitude_velocity() // 14 accel based lon velocity
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif
} }
// Read an INAV packet // Read an INAV packet
@ -819,11 +815,10 @@ static void Log_Read_INAV()
struct log_INAV pkt; struct log_INAV pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt)); DataFlash.ReadPacket(&pkt, sizeof(pkt));
// 1 2 3 4 5 6 7 8 9 10 11 12 13 14 // 1 2 3 4 5 6 7 8 9 10 11 12 13
cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"), cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"),
(int)pkt.baro_alt, // 1 barometer altitude (int)pkt.baro_alt, // 1 barometer altitude
(int)pkt.inav_alt, // 2 accel + baro filtered altitude (int)pkt.inav_alt, // 2 accel + baro filtered altitude
(int)pkt.baro_climb_rate, // 3 barometer based climb rate
(int)pkt.inav_climb_rate, // 4 accel + baro based climb rate (int)pkt.inav_climb_rate, // 4 accel + baro based climb rate
(float)pkt.accel_corr_x, // 5 accel correction x-axis (float)pkt.accel_corr_x, // 5 accel correction x-axis
(float)pkt.accel_corr_y, // 6 accel correction y-axis (float)pkt.accel_corr_y, // 6 accel correction y-axis

2
ArduCopter/Parameters.pde
View File

@ -977,11 +977,9 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(ins, "INS_", AP_InertialSensor), GOBJECT(ins, "INS_", AP_InertialSensor),
#endif #endif
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
// @Group: INAV_ // @Group: INAV_
// @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp // @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp
GOBJECT(inertial_nav, "INAV_", AP_InertialNav), GOBJECT(inertial_nav, "INAV_", AP_InertialNav),
#endif
// @Group: SR0_ // @Group: SR0_
// @Path: ./GCS_Mavlink.pde // @Path: ./GCS_Mavlink.pde

3
ArduCopter/config.h
View File

@ -1281,8 +1281,5 @@
#ifndef INERTIAL_NAV_XY #ifndef INERTIAL_NAV_XY
# define INERTIAL_NAV_XY DISABLED # define INERTIAL_NAV_XY DISABLED
#endif #endif
#ifndef INERTIAL_NAV_Z
# define INERTIAL_NAV_Z ENABLED
#endif
#endif // __ARDUCOPTER_CONFIG_H__ #endif // __ARDUCOPTER_CONFIG_H__

10
ArduCopter/inertia.pde
View File

@ -3,7 +3,6 @@
// read_inertia - read inertia in from accelerometers // read_inertia - read inertia in from accelerometers
static void read_inertia() static void read_inertia()
{ {
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
static uint8_t log_counter_inav = 0; static uint8_t log_counter_inav = 0;
// inertial altitude estimates // inertial altitude estimates
@ -16,5 +15,12 @@ static void read_inertia()
Log_Write_INAV(); Log_Write_INAV();
} }
} }
#endif }
// read_inertial_altitude - pull altitude and climb rate from inertial nav library
static void read_inertial_altitude()
{
// 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();
} }

2
ArduCopter/motors.pde
View File

@ -187,9 +187,7 @@ static void init_disarm_motors()
g.throttle_cruise.save(); g.throttle_cruise.save();
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
inertial_nav.save_params(); inertial_nav.save_params();
#endif
// we are not in the air // we are not in the air
set_takeoff_complete(false); set_takeoff_complete(false);

1
ArduCopter/navigation.pde
View File

@ -89,6 +89,7 @@ static void run_nav_updates(void)
{ {
if (nav_updates.need_velpos) { if (nav_updates.need_velpos) {
calc_velocity_and_position(); calc_velocity_and_position();
verify_altitude();
nav_updates.need_velpos = 0; nav_updates.need_velpos = 0;
} else if (nav_updates.need_dist_bearing) { } else if (nav_updates.need_dist_bearing) {
calc_distance_and_bearing(); calc_distance_and_bearing();

2
ArduCopter/system.pde
View File

@ -227,10 +227,8 @@ static void init_ardupilot()
init_optflow(); init_optflow();
} }
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
// initialise inertial nav // initialise inertial nav
inertial_nav.init(); inertial_nav.init();
#endif
#ifdef USERHOOK_INIT #ifdef USERHOOK_INIT
USERHOOK_INIT USERHOOK_INIT