diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index dddc0f4598..4d17ce029a 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -48,8 +48,6 @@ #define NAV_LOITER_ACTIVE NAV_LOITER_INAV #define RTL_YAW YAW_HOLD -#define INERTIAL_NAV_Z ENABLED - //#define MOTORS_JD880 //#define MOTORS_JD850 diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 87be304c40..88814aaf60 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -648,21 +648,13 @@ static float current_total1; static float controller_desired_alt; // The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP 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 static int16_t climb_rate; // The altitude as reported by Sonar in cm – Values are 20 to 700 generally. static int16_t sonar_alt; 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 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; @@ -827,9 +819,7 @@ static float G_Dt = 0.02; //////////////////////////////////////////////////////////////////////////////// // Inertial Navigation //////////////////////////////////////////////////////////////////////////////// -#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps); -#endif //////////////////////////////////////////////////////////////////////////////// // Performance monitoring @@ -931,7 +921,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { update_GPS, 2, 900 }, { update_navigation, 2, 500 }, { medium_loop, 2, 700 }, - { update_altitude_est, 2, 1000 }, + { update_altitude, 5, 1000 }, { fifty_hz_loop, 2, 950 }, { run_nav_updates, 2, 500 }, { slow_loop, 10, 500 }, @@ -1225,6 +1215,9 @@ static void medium_loop() // --------------------------- static void fifty_hz_loop() { + // get altitude and climb rate from inertial lib + read_inertial_altitude(); + // Update the throttle ouput // ------------------------- update_throttle_mode(); @@ -2078,107 +2071,28 @@ static void update_trig(void){ // 270 = cos_yaw: -1.00, sin_yaw: 0.00, } -// updated at 10hz +// read baro and sonar altitude at 10hz static void update_altitude() { - 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; - baro_rate = (baro_alt - old_baro_alt) * 5; // 5hz + baro_alt = g_gps->altitude - gps_base_alt; + if(g.sonar_enabled) { - sonar_alt = fake_relative_alt; - sonar_rate = baro_rate; + sonar_alt = baro_alt; } - current_loc.alt = baro_alt; - climb_rate_actual = baro_rate; #else - // read in actual baro altitude + // read in 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 + // read in sonar altitude 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 - // update the target altitude - 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()) { - 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); + // write altitude info to dataflash logs + if ((g.log_bitmask & MASK_LOG_CTUN) && motors.armed()) { + Log_Write_Control_Tuning(); } -#endif } static void tuning(){ @@ -2331,9 +2245,7 @@ static void tuning(){ #if INERTIAL_NAV_XY == ENABLED inertial_nav.set_time_constant_xy(tuning_value); #endif -#if INERTIAL_NAV_Z == ENABLED inertial_nav.set_time_constant_z(tuning_value); -#endif break; case CH6_THR_ACCEL_KP: diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index b0042e477a..28e3d7a5bc 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -772,7 +772,6 @@ struct log_INAV { LOG_PACKET_HEADER; int16_t baro_alt; int16_t inav_alt; - int16_t baro_climb_rate; int16_t inav_climb_rate; float accel_corr_x; float accel_corr_y; @@ -789,28 +788,25 @@ struct log_INAV { // Write an INAV packet. Total length : 52 Bytes static void Log_Write_INAV() { -#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED Vector3f accel_corr = inertial_nav.accel_correction_ef; struct log_INAV pkt = { LOG_PACKET_HEADER_INIT(LOG_INAV_MSG), baro_alt : (int16_t)baro_alt, // 1 barometer 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(), // 4 accel + baro based climb rate - accel_corr_x : accel_corr.x, // 5 accel correction x-axis - accel_corr_y : accel_corr.y, // 6 accel correction y-axis - accel_corr_z : accel_corr.z, // 7 accel correction z-axis - accel_corr_ef_z : inertial_nav.accel_correction_ef.z, // 8 accel correction earth frame - gps_lat_from_home : g_gps->latitude-home.lat, // 9 lat from home - gps_lon_from_home : g_gps->longitude-home.lng, // 10 lon 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(), // 12 accel based lon from home - inav_lat_speed : inertial_nav.get_latitude_velocity(), // 13 accel based lat velocity - inav_lon_speed : inertial_nav.get_longitude_velocity() // 14 accel based lon velocity + inav_climb_rate : (int16_t)inertial_nav.get_velocity_z(), // 3 accel + baro based climb rate + accel_corr_x : accel_corr.x, // 4 accel correction x-axis + accel_corr_y : accel_corr.y, // 5 accel correction y-axis + accel_corr_z : accel_corr.z, // 6 accel correction z-axis + accel_corr_ef_z : inertial_nav.accel_correction_ef.z, // 7 accel correction earth frame + gps_lat_from_home : g_gps->latitude-home.lat, // 8 lat from home + gps_lon_from_home : g_gps->longitude-home.lng, // 9 lon from home + inav_lat_from_home : inertial_nav.get_latitude_diff(), // 10 accel based lat from home + inav_lon_from_home : inertial_nav.get_longitude_diff(), // 11 accel based lon from home + inav_lat_speed : inertial_nav.get_latitude_velocity(), // 12 accel based lat velocity + inav_lon_speed : inertial_nav.get_longitude_velocity() // 13 accel based lon velocity }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); -#endif } // Read an INAV packet @@ -819,11 +815,10 @@ static void Log_Read_INAV() struct log_INAV pkt; DataFlash.ReadPacket(&pkt, sizeof(pkt)); - // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 - 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"), + // 1 2 3 4 5 6 7 8 9 10 11 12 13 + 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.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 (float)pkt.accel_corr_x, // 5 accel correction x-axis (float)pkt.accel_corr_y, // 6 accel correction y-axis diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index e444f2cb28..ce0c7b9d40 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -977,11 +977,9 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(ins, "INS_", AP_InertialSensor), #endif -#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED // @Group: INAV_ // @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp GOBJECT(inertial_nav, "INAV_", AP_InertialNav), -#endif // @Group: SR0_ // @Path: ./GCS_Mavlink.pde diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2d5e9569f7..e1becb8185 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -1281,8 +1281,5 @@ #ifndef INERTIAL_NAV_XY # define INERTIAL_NAV_XY DISABLED #endif -#ifndef INERTIAL_NAV_Z - # define INERTIAL_NAV_Z ENABLED -#endif #endif // __ARDUCOPTER_CONFIG_H__ diff --git a/ArduCopter/inertia.pde b/ArduCopter/inertia.pde index 7892788f4c..ce951fbb0a 100644 --- a/ArduCopter/inertia.pde +++ b/ArduCopter/inertia.pde @@ -3,7 +3,6 @@ // read_inertia - read inertia in from accelerometers static void read_inertia() { -#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED static uint8_t log_counter_inav = 0; // inertial altitude estimates @@ -16,5 +15,12 @@ static void read_inertia() 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(); } \ No newline at end of file diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index dfb9b782b2..49a6eca29e 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -187,9 +187,7 @@ static void init_disarm_motors() g.throttle_cruise.save(); -#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED inertial_nav.save_params(); -#endif // we are not in the air set_takeoff_complete(false); diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index e6aa8996e1..9b25c2207e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -89,6 +89,7 @@ static void run_nav_updates(void) { if (nav_updates.need_velpos) { calc_velocity_and_position(); + verify_altitude(); nav_updates.need_velpos = 0; } else if (nav_updates.need_dist_bearing) { calc_distance_and_bearing(); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index ae7fcaa9d9..68715ec389 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -227,10 +227,8 @@ static void init_ardupilot() init_optflow(); } -#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED // initialise inertial nav inertial_nav.init(); -#endif #ifdef USERHOOK_INIT USERHOOK_INIT