Remove INAV from Position Control

ArduCopter/mode.cpp

@@ -405,6 +405,7 @@ bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason)
 // called at 100hz or more
 void Copter::update_flight_mode()
 {
+    pos_control->update_estimates(vibration_check.high_vibes);
 #if AP_RANGEFINDER_ENABLED
     surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used
 #endif
@@ -654,7 +655,7 @@ void Mode::land_run_vertical_control(bool pause_descent)
             Vector2f target_pos;
             float target_error_cm = 0.0f;
             if (copter.precland.get_target_position_cm(target_pos)) {
-                const Vector2f current_pos = inertial_nav.get_position_xy_cm();
+                const Vector2f current_pos = pos_control->get_pos_estimate_NEU_cm().xy().tofloat();
                 // target is this many cm away from the vehicle
                 target_error_cm = (target_pos - current_pos).length();
             }
@@ -738,10 +739,10 @@ void Mode::land_run_horizontal_control()
     if (copter.ap.prec_land_active) {
         Vector2f target_pos, target_vel;
         if (!copter.precland.get_target_position_cm(target_pos)) {
-            target_pos = inertial_nav.get_position_xy_cm();
+            target_pos = pos_control->get_pos_estimate_NEU_cm().xy().tofloat();
         }
          // get the velocity of the target
-        copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel);
+        copter.precland.get_target_velocity_cms(pos_control->get_vel_estimate_NEU_cms().xy(), target_vel);
 
         Vector2f zero;
         Vector2p landing_pos = target_pos.topostype();

ArduCopter/mode_auto.cpp

@@ -378,7 +378,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
     // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin
     int32_t alt_target_cm;
     bool alt_target_terrain = false;
-    float current_alt_cm = inertial_nav.get_position_z_up_cm();
+    float current_alt_cm = pos_control->get_pos_estimate_NEU_cm().tofloat().z;
     float terrain_offset;   // terrain's altitude in cm above the ekf origin
     if ((dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) && wp_nav->get_terrain_offset_cm(terrain_offset)) {
         // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
@@ -535,7 +535,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
         }
 
         // if we are outside the circle, point at the edge, otherwise hold yaw
-        const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), copter.circle_nav->get_center_NEU_cm().xy());
+        const float dist_to_center = get_horizontal_distance_cm(pos_control->get_pos_estimate_NEU_cm().tofloat().xy(), copter.circle_nav->get_center_NEU_cm().tofloat().xy());
         // initialise yaw
         // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
         if (auto_yaw.mode() != AutoYaw::Mode::ROI) {

ArduCopter/mode_brake.cpp

@@ -10,8 +10,8 @@
 bool ModeBrake::init(bool ignore_checks)
 {
     // initialise pos controller speed and acceleration
-    pos_control->set_max_speed_accel_NE_cm(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE);
-    pos_control->set_correction_speed_accel_NE_cm(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE);
+    pos_control->set_max_speed_accel_NE_cm(pos_control->get_vel_estimate_NEU_cms().length(), BRAKE_MODE_DECEL_RATE);
+    pos_control->set_correction_speed_accel_NE_cm(pos_control->get_vel_estimate_NEU_cms().length(), BRAKE_MODE_DECEL_RATE);
 
     // initialise position controller
     pos_control->init_NE_controller();

ArduCopter/mode_drift.cpp

@@ -46,7 +46,7 @@ void ModeDrift::run()
     get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
 
     // Grab inertial velocity
-    const Vector3f& vel = inertial_nav.get_velocity_neu_cms();
+    const Vector3f& vel = pos_control->get_vel_estimate_NEU_cms();
 
     // rotate roll, pitch input from north facing to vehicle's perspective
     float roll_vel =  vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel

ArduCopter/mode_flowhold.cpp

@@ -106,7 +106,7 @@ bool ModeFlowHold::init(bool ignore_checks)
     flow_pi_xy.set_dt(1.0/copter.scheduler.get_loop_rate_hz());
 
     // start with INS height
-    last_ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
+    last_ins_height = pos_control->get_pos_estimate_NEU_cm().tofloat().z * 0.01;
     height_offset = 0;
 
     return true;
@@ -130,7 +130,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input)
     Vector2f sensor_flow = flow_filter.apply(raw_flow);
 
     // scale by height estimate, limiting it to height_min to height_max
-    float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
+    float ins_height = pos_control->get_pos_estimate_NEU_cm().tofloat().z * 0.01;
     float height_estimate = ins_height + height_offset;
 
     // compensate for height, this converts to (approx) m/s
@@ -351,7 +351,7 @@ void ModeFlowHold::run()
  */
 void ModeFlowHold::update_height_estimate(void)
 {
-    float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
+    float ins_height = copter.pos_control->get_pos_estimate_NEU_cm().tofloat().z * 0.01;
 
 #if 1
     // assume on ground when disarmed, or if we have only just started spooling the motors up

ArduCopter/mode_guided.cpp

@@ -1062,7 +1062,7 @@ void ModeGuided::limit_init_time_and_pos()
     guided_limit.start_time = AP_HAL::millis();
 
     // initialise start position from current position
-    guided_limit.start_pos = inertial_nav.get_position_neu_cm();
+    guided_limit.start_pos = pos_control->get_pos_estimate_NEU_cm().tofloat();
 }
 
 // limit_check - returns true if guided mode has breached a limit
@@ -1075,7 +1075,7 @@ bool ModeGuided::limit_check()
     }
 
     // get current location
-    const Vector3f& curr_pos = inertial_nav.get_position_neu_cm();
+    const Vector3f& curr_pos = pos_control->get_pos_estimate_NEU_cm().tofloat();
 
     // check if we have gone below min alt
     if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) {
@@ -1120,7 +1120,7 @@ float ModeGuided::wp_distance_m() const
     case SubMode::WP:
         return wp_nav->get_wp_distance_to_destination_cm() * 0.01f;
     case SubMode::Pos:
-        return get_horizontal_distance_cm(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()) * 0.01f;
+        return get_horizontal_distance_cm(pos_control->get_pos_estimate_NEU_cm().tofloat().xy(), guided_pos_target_cm.tofloat().xy()) * 0.01f;
     case SubMode::PosVelAccel:
         return pos_control->get_pos_error_NE_cm() * 0.01f;
     default:
@@ -1134,7 +1134,7 @@ int32_t ModeGuided::wp_bearing() const
     case SubMode::WP:
         return wp_nav->get_wp_bearing_to_destination_cd();
     case SubMode::Pos:
-        return get_bearing_cd(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy());
+        return get_bearing_cd(pos_control->get_pos_estimate_NEU_cm().tofloat().xy(), guided_pos_target_cm.tofloat().xy());
     case SubMode::PosVelAccel:
         return pos_control->get_bearing_to_target_cd();
     case SubMode::TakeOff:

ArduCopter/mode_loiter.cpp

@@ -65,10 +65,10 @@ void ModeLoiter::precision_loiter_xy()
     loiter_nav->clear_pilot_desired_acceleration();
     Vector2f target_pos, target_vel;
     if (!copter.precland.get_target_position_cm(target_pos)) {
-        target_pos = inertial_nav.get_position_xy_cm();
+        target_pos = pos_control->get_pos_estimate_NEU_cm().tofloat().xy();
     }
     // get the velocity of the target
-    copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel);
+    copter.precland.get_target_velocity_cms(pos_control->get_vel_estimate_NEU_cms().xy(), target_vel);
 
     Vector2f zero;
     Vector2p landing_pos = target_pos.topostype();

ArduCopter/mode_poshold.cpp

@@ -69,7 +69,7 @@ void ModePosHold::run()
 {
     float controller_to_pilot_roll_mix; // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
     float controller_to_pilot_pitch_mix;    // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
-    const Vector3f& vel = inertial_nav.get_velocity_neu_cms();
+    const Vector3f& vel = pos_control->get_vel_estimate_NEU_cms();
 
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_U_cm(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@@ -377,7 +377,7 @@ void ModePosHold::run()
         pitch_mode = RPMode::BRAKE_TO_LOITER;
         brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
         // init loiter controller
-        loiter_nav->init_target_cm(inertial_nav.get_position_xy_cm() - pos_control->get_pos_offset_NEU_cm().xy().tofloat());
+        loiter_nav->init_target_cm((pos_control->get_pos_estimate_NEU_cm().xy() - pos_control->get_pos_offset_NEU_cm().xy()).tofloat());
         // set delay to start of wind compensation estimate updates
         wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
     }
@@ -564,7 +564,7 @@ void ModePosHold::update_wind_comp_estimate()
     }
 
     // check horizontal velocity is low
-    if (inertial_nav.get_speed_xy_cms() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) {
+    if (pos_control->get_vel_estimate_NEU_cms().xy().length() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) {
         return;
     }
 

ArduCopter/mode_rtl.cpp

@@ -309,7 +309,7 @@ void ModeRTL::descent_run()
     attitude_control->input_thrust_vector_heading_cd(pos_control->get_thrust_vector(), auto_yaw.get_heading());
 
     // check if we've reached within 20cm of final altitude
-    _state_complete = labs(rtl_path.descent_target.alt - copter.inertial_nav.get_position_z_up_cm()) < 20;
+    _state_complete = labs(rtl_path.descent_target.alt - pos_control->get_pos_estimate_NEU_cm().tofloat().z) < 20;
 }
 
 // land_start - initialise controllers to loiter over home

ArduCopter/mode_systemid.cpp

@@ -129,7 +129,7 @@ bool ModeSystemId::init(bool ignore_checks)
             pos_control->init_U_controller();
         }
         Vector3f curr_pos;
-        curr_pos = inertial_nav.get_position_neu_cm();
+        curr_pos = pos_control->get_pos_estimate_NEU_cm().tofloat();
         target_pos = curr_pos.xy();
     }
 

ArduCopter/mode_throw.cpp

@@ -72,9 +72,9 @@ void ModeThrow::run()
         // initialise the demanded height below/above the throw height from user parameters
         // this allows for rapidly clearing surrounding obstacles
         if (g2.throw_type == ThrowType::Drop) {
-            pos_control->set_pos_desired_U_cm(inertial_nav.get_position_z_up_cm() - g.throw_altitude_descend * 100.0f);
+            pos_control->set_pos_desired_U_cm(pos_control->get_pos_estimate_NEU_cm().z - g.throw_altitude_descend * 100.0f);
         } else {
-            pos_control->set_pos_desired_U_cm(inertial_nav.get_position_z_up_cm() + g.throw_altitude_ascend * 100.0f);
+            pos_control->set_pos_desired_U_cm(pos_control->get_pos_estimate_NEU_cm().z + g.throw_altitude_ascend * 100.0f);
         }
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
@@ -206,8 +206,8 @@ void ModeThrow::run()
     if ((stage != prev_stage) || (now - last_log_ms) > 100) {
         prev_stage = stage;
         last_log_ms = now;
-        const float velocity = inertial_nav.get_velocity_neu_cms().length();
-        const float velocity_z = inertial_nav.get_velocity_z_up_cms();
+        const float velocity = pos_control->get_vel_estimate_NEU_cms().length();
+        const float velocity_z = pos_control->get_vel_estimate_NEU_cms().z;
         const float accel = copter.ins.get_accel().length();
         const float ef_accel_z = ahrs.get_accel_ef().z;
         const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
@@ -258,14 +258,14 @@ bool ModeThrow::throw_detected()
     }
 
     // Check for high speed (>500 cm/s)
-    bool high_speed = inertial_nav.get_velocity_neu_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED);
+    bool high_speed = pos_control->get_vel_estimate_NEU_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED);
 
     // check for upwards or downwards trajectory (airdrop) of 50cm/s
     bool changing_height;
     if (g2.throw_type == ThrowType::Drop) {
-        changing_height = inertial_nav.get_velocity_z_up_cms() < -THROW_VERTICAL_SPEED;
+        changing_height = pos_control->get_vel_estimate_NEU_cms().z < -THROW_VERTICAL_SPEED;
     } else {
-        changing_height = inertial_nav.get_velocity_z_up_cms() > THROW_VERTICAL_SPEED;
+        changing_height = pos_control->get_vel_estimate_NEU_cms().z > THROW_VERTICAL_SPEED;
     }
 
     // Check the vertical acceleration is greater than 0.25g
@@ -280,7 +280,7 @@ bool ModeThrow::throw_detected()
         ahrs.get_relative_position_D_home(altitude_above_home);
         altitude_above_home = -altitude_above_home; // altitude above home is returned as negative
     } else {
-        altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters
+        altitude_above_home = pos_control->get_pos_estimate_NEU_cm().z * 0.01f; // centimeters to meters
     }
 
     // Check that the altitude is within user defined limits
@@ -293,11 +293,11 @@ bool ModeThrow::throw_detected()
     // Record time and vertical velocity when we detect the possible throw
     if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
         free_fall_start_ms = AP_HAL::millis();
-        free_fall_start_velz = inertial_nav.get_velocity_z_up_cms();
+        free_fall_start_velz = pos_control->get_vel_estimate_NEU_cms().z;
     }
 
     // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
-    bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f));
+    bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((pos_control->get_vel_estimate_NEU_cms().z - free_fall_start_velz) < -250.0f));
 
     // start motors and enter the control mode if we are in continuous freefall
     return throw_condition_confirmed;

ArduCopter/system.cpp

@@ -448,30 +448,30 @@ void Copter::allocate_motors(void)
     }
     AP_Param::load_object_from_eeprom(attitude_control, attitude_control_var_info);
 
-    pos_control = NEW_NOTHROW AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
+    pos_control = NEW_NOTHROW AC_PosControl(*ahrs_view, *motors, *attitude_control);
     if (pos_control == nullptr) {
         AP_BoardConfig::allocation_error("PosControl");
     }
     AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info);
 
 #if AP_OAPATHPLANNER_ENABLED
-    wp_nav = NEW_NOTHROW AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+    wp_nav = NEW_NOTHROW AC_WPNav_OA(*ahrs_view, *pos_control, *attitude_control);
 #else
-    wp_nav = NEW_NOTHROW AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+    wp_nav = NEW_NOTHROW AC_WPNav(*ahrs_view, *pos_control, *attitude_control);
 #endif
     if (wp_nav == nullptr) {
         AP_BoardConfig::allocation_error("WPNav");
     }
     AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info);
 
-    loiter_nav = NEW_NOTHROW AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
+    loiter_nav = NEW_NOTHROW AC_Loiter(*ahrs_view, *pos_control, *attitude_control);
     if (loiter_nav == nullptr) {
         AP_BoardConfig::allocation_error("LoiterNav");
     }
     AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
 
 #if MODE_CIRCLE_ENABLED
-    circle_nav = NEW_NOTHROW AC_Circle(inertial_nav, *ahrs_view, *pos_control);
+    circle_nav = NEW_NOTHROW AC_Circle(*ahrs_view, *pos_control);
     if (circle_nav == nullptr) {
         AP_BoardConfig::allocation_error("CircleNav");
     }

ArduCopter/takeoff.cpp

@@ -115,7 +115,6 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
 void _AutoTakeoff::run()
 {
     const auto &g2 = copter.g2;
-    const auto &inertial_nav = copter.inertial_nav;
     const auto &wp_nav = copter.wp_nav;
     auto *motors = copter.motors;
     auto *pos_control = copter.pos_control;
@@ -126,7 +125,7 @@ void _AutoTakeoff::run()
         // do not spool down tradheli when on the ground with motor interlock enabled
         copter.flightmode->make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
         // update auto_takeoff_no_nav_alt_cm
-        no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+        no_nav_alt_cm = pos_control->get_pos_estimate_NEU_cm().z + g2.wp_navalt_min * 100;
         return;
     }
 
@@ -152,7 +151,7 @@ void _AutoTakeoff::run()
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->input_thrust_vector_rate_heading_cds(pos_control->get_thrust_vector(), 0.0);
         // update auto_takeoff_no_nav_alt_cm
-        no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+        no_nav_alt_cm = pos_control->get_pos_estimate_NEU_cm().z + g2.wp_navalt_min * 100;
         return;
     }
 
@@ -170,7 +169,7 @@ void _AutoTakeoff::run()
         if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || 
             (copter.pos_control->get_measured_accel_U_cmss() >= 0.5 * copter.pos_control->get_max_accel_U_cmss()) ||
             (copter.pos_control->get_vel_desired_NEU_cms().z >= 0.1 * copter.pos_control->get_max_speed_up_cms()) || 
-            ( no_nav_active && (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm))) {
+            ( no_nav_active && (pos_control->get_pos_estimate_NEU_cm().z >= no_nav_alt_cm))) {
             // throttle > 90%
             // acceleration > 50% maximum acceleration
             // velocity > 10% maximum velocity
@@ -183,7 +182,7 @@ void _AutoTakeoff::run()
     // check if we are not navigating because of low altitude
     if (no_nav_active) {
         // check if vehicle has reached no_nav_alt threshold
-        if (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm) {
+        if (pos_control->get_pos_estimate_NEU_cm().z >= no_nav_alt_cm) {
             no_nav_active = false;
         }
         pos_control->relax_velocity_controller_NE();
@@ -229,8 +228,7 @@ void _AutoTakeoff::start(float _complete_alt_cm, bool _terrain_alt)
     complete = false;
     // initialise auto_takeoff_no_nav_alt_cm
     const auto &g2 = copter.g2;
-    const auto &inertial_nav = copter.inertial_nav;
-    no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+    no_nav_alt_cm = copter.pos_control->get_pos_estimate_NEU_cm().z + g2.wp_navalt_min * 100;
     if ((g2.wp_navalt_min > 0) && (copter.flightmode->is_disarmed_or_landed() || !copter.motors->get_interlock())) {
         // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
         no_nav_active = true;

ArduPlane/Plane.cpp

@@ -204,6 +204,9 @@ void Plane::ahrs_update()
 
     // update inertial_nav for quadplane
     quadplane.inertial_nav.update();
+    if (quadplane.available()) {  
+        quadplane.pos_control->update_estimates();  
+    }  
 #endif
 
 #if HAL_LOGGING_ENABLED