diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 094f5dbfe9..009619f786 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -602,11 +602,7 @@ static float G_Dt = 0.02; //////////////////////////////////////////////////////////////////////////////// // Inertial Navigation //////////////////////////////////////////////////////////////////////////////// -#if AP_AHRS_NAVEKF_AVAILABLE static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch); -#else -static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch); -#endif //////////////////////////////////////////////////////////////////////////////// // Attitude, Position and Waypoint navigation objects diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index b9edaca786..5cf846cef6 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -339,7 +339,6 @@ struct PACKED log_Performance { int16_t pm_test; uint8_t i2c_lockup_count; uint16_t ins_error_count; - uint8_t inav_error_count; }; // Write a performance monitoring packet @@ -352,8 +351,7 @@ static void Log_Write_Performance() max_time : perf_info_get_max_time(), pm_test : pmTest1, i2c_lockup_count : hal.i2c->lockup_count(), - ins_error_count : ins.error_count(), - inav_error_count : inertial_nav.error_count() + ins_error_count : ins.error_count() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -607,7 +605,7 @@ static const struct LogStructure log_structure[] PROGMEM = { { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), "CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" }, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), - "PM", "HHIhBHB", "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" }, + "PM", "HHIhBH", "NLon,NLoop,MaxT,PMT,I2CErr,INSErr" }, { LOG_RATE_MSG, sizeof(log_Rate), "RATE", "Ifffffffff", "TimeMS,RllDes,Rll,RllOut,PitDes,Pit,PitOut,YawDes,Yaw,YawOut" }, { LOG_MOT_MSG, sizeof(log_Mot), diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index bfa681ea4c..cbea8e7a37 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -167,7 +167,7 @@ public: // // 100: Inertial Nav // - k_param_inertial_nav = 100, + k_param_inertial_nav = 100, // deprecated k_param_wp_nav, k_param_attitude_control, k_param_pos_control, diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index f94ab492cc..e100fa33b0 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -810,10 +810,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp GOBJECT(ins, "INS_", AP_InertialSensor), - // @Group: INAV_ - // @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp - GOBJECT(inertial_nav, "INAV_", AP_InertialNav), - // @Group: WPNAV_ // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp GOBJECT(wp_nav, "WPNAV_", AC_WPNav), diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index c1a6df3da6..3b256662a1 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -87,8 +87,6 @@ static bool set_home(const Location& loc) if (g.compass_enabled) { compass.set_initial_location(gps.location().lat, gps.location().lng); } - // set inertial nav home - inertial_nav.setup_home_position(); // update navigation scalers. used to offset the shrinking longitude as we go towards the poles scaleLongDown = longitude_scale(loc); scaleLongUp = 1.0f/scaleLongDown; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 083d6ba708..044731c184 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -117,9 +117,6 @@ static bool init_arm_motors(bool arming_from_gcs) // disable cpu failsafe because initialising everything takes a while failsafe_disable(); - // disable inertial nav errors temporarily - inertial_nav.ignore_next_error(); - // reset battery failsafe set_failsafe_battery(false); @@ -162,9 +159,6 @@ static bool init_arm_motors(bool arming_from_gcs) // fast baro calibration to reset ground pressure init_barometer(false); - // reset inertial nav alt to zero - inertial_nav.set_altitude(0.0f); - // go back to normal AHRS gains ahrs.set_fast_gains(false); @@ -688,9 +682,6 @@ static void init_disarm_motors() motors.armed(false); - // disable inertial nav errors temporarily - inertial_nav.ignore_next_error(); - // save offsets if automatic offset learning is on if (compass.learn_offsets_enabled()) { compass.save_offsets(); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 57776acded..7e55d2aea2 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -208,9 +208,6 @@ static void init_ardupilot() // init the optical flow sensor init_optflow(); - // initialise inertial nav - inertial_nav.init(); - // initialise camera mount camera_mount.init(serial_manager);