From 3107c42fca75a66ce98169c774de64c4376e2d80 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Wed, 20 Oct 2021 04:29:57 -0400 Subject: [PATCH] Copter: INAV rename for neu & cm/cms --- ArduCopter/AP_Arming.cpp | 4 ++-- ArduCopter/Attitude.cpp | 2 +- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/Log.cpp | 4 ++-- ArduCopter/autoyaw.cpp | 8 ++++---- ArduCopter/baro_ground_effect.cpp | 8 ++++---- ArduCopter/crash_check.cpp | 4 ++-- ArduCopter/heli.cpp | 2 +- ArduCopter/inertia.cpp | 2 +- ArduCopter/land_detector.cpp | 2 +- ArduCopter/mode.cpp | 6 +++--- ArduCopter/mode_auto.cpp | 12 ++++++------ ArduCopter/mode_autorotate.cpp | 2 +- ArduCopter/mode_brake.cpp | 4 ++-- ArduCopter/mode_drift.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 6 +++--- ArduCopter/mode_follow.cpp | 4 ++-- ArduCopter/mode_guided.cpp | 8 ++++---- ArduCopter/mode_loiter.cpp | 4 ++-- ArduCopter/mode_poshold.cpp | 6 +++--- ArduCopter/mode_throw.cpp | 18 +++++++++--------- ArduCopter/mode_zigzag.cpp | 10 +++++----- ArduCopter/sensors.cpp | 4 ++-- ArduCopter/surface_tracking.cpp | 4 ++-- ArduCopter/takeoff.cpp | 4 ++-- ArduCopter/toy_mode.cpp | 2 +- 26 files changed, 67 insertions(+), 67 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 301be41414..096cdeb9af 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -81,7 +81,7 @@ bool AP_Arming_Copter::barometer_checks(bool display_failure) nav_filter_status filt_status = copter.inertial_nav.get_filter_status(); bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); if (using_baro_ref) { - if (fabsf(copter.inertial_nav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { + if (fabsf(copter.inertial_nav.get_position_z_up_cm() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity"); ret = false; } @@ -816,7 +816,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ } // remember the height when we armed - copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01; + copter.arming_altitude_m = copter.inertial_nav.get_position_z_up_cm() * 0.01; } copter.update_super_simple_bearing(false); diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index ddace6d25f..c823849d86 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -27,7 +27,7 @@ void Copter::update_throttle_hover() float throttle = motors->get_throttle(); // calc average throttle if we are in a level hover. accounts for heli hover roll trim - if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z()) < 60 && + if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z_up_cms()) < 60 && labs(ahrs.roll_sensor-attitude_control->get_roll_trim_cd()) < 500 && labs(ahrs.pitch_sensor) < 500) { // Can we set the time constant automatically motors->update_throttle_hover(0.01f); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a4a991cee1..e3a8786ac6 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1179,7 +1179,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED || packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { - pos_vector += copter.inertial_nav.get_position(); + pos_vector += copter.inertial_nav.get_position_neu_cm(); } } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index b62e75adb6..1b5582dd48 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -53,13 +53,13 @@ void Copter::Log_Write_Control_Tuning() throttle_out : motors->get_throttle(), throttle_hover : motors->get_throttle_hover(), desired_alt : des_alt_m, - inav_alt : inertial_nav.get_altitude() / 100.0f, + inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : baro_alt, desired_rangefinder_alt : desired_rangefinder_alt, rangefinder_alt : surface_tracking.get_dist_for_logging(), terr_alt : terr_alt, target_climb_rate : target_climb_rate_cms, - climb_rate : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t + climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()) // float -> int16_t }; logger.WriteBlock(&pkt, sizeof(pkt)); } diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index c06b944d82..98e2dfd2d9 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -5,15 +5,15 @@ Mode::AutoYaw Mode::auto_yaw; // roi_yaw - returns heading towards location held in roi float Mode::AutoYaw::roi_yaw() const { - return get_bearing_cd(copter.inertial_nav.get_position_xy(), roi.xy()); + return get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy()); } float Mode::AutoYaw::look_ahead_yaw() { - const Vector3f& vel = copter.inertial_nav.get_velocity(); - float speed = vel.xy().length(); + const Vector3f& vel = copter.inertial_nav.get_velocity_neu_cms(); + const float speed_sq = vel.xy().length_squared(); // Commanded Yaw to automatically look ahead. - if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) { + if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) { _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f; } return _look_ahead_yaw; diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp index 8b3aa397cc..3333fb36e8 100644 --- a/ArduCopter/baro_ground_effect.cpp +++ b/ArduCopter/baro_ground_effect.cpp @@ -24,7 +24,7 @@ void Copter::update_ground_effect_detector(void) } if (position_ok() || ekf_has_relative_position()) { - Vector3f vel = inertial_nav.get_velocity(); + Vector3f vel = inertial_nav.get_velocity_neu_cms(); vel.z = 0.0f; xy_speed_cms = vel.length(); } @@ -43,12 +43,12 @@ void Copter::update_ground_effect_detector(void) const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->get_control_in() > 0; if (!throttle_up && ap.land_complete) { gndeffect_state.takeoff_time_ms = tnow_ms; - gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude(); + gndeffect_state.takeoff_alt_cm = inertial_nav.get_position_z_up_cm(); } // if we are in takeoff_expected and we meet the conditions for having taken off // end the takeoff_expected state - if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) { + if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_position_z_up_cm()-gndeffect_state.takeoff_alt_cm > 50.0f)) { gndeffect_state.takeoff_expected = false; } @@ -61,7 +61,7 @@ void Copter::update_ground_effect_detector(void) bool descent_demanded = pos_control->is_active_z() && des_climb_rate_cms < 0.0f; bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f; - bool z_speed_low = fabsf(inertial_nav.get_velocity_z()) <= 60.0f; + bool z_speed_low = fabsf(inertial_nav.get_velocity_z_up_cms()) <= 60.0f; bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded)); gndeffect_state.touchdown_expected = slow_horizontal && slow_descent; diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index db115b06b1..d000f01a7b 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -137,7 +137,7 @@ void Copter::thrust_loss_check() } // check for descent - if (!is_negative(inertial_nav.get_velocity_z())) { + if (!is_negative(inertial_nav.get_velocity_z_up_cms())) { thrust_loss_counter = 0; return; } @@ -240,7 +240,7 @@ void Copter::parachute_check() parachute.set_is_flying(!ap.land_complete); // pass sink rate to parachute library - parachute.set_sink_rate(-inertial_nav.get_velocity_z() * 0.01f); + parachute.set_sink_rate(-inertial_nav.get_velocity_z_up_cms() * 0.01f); // exit immediately if in standby if (standby_active) { diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 5366118c2f..2118edc330 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -35,7 +35,7 @@ void Copter::check_dynamic_flight(void) // with GPS lock use inertial nav to determine if we are moving if (position_ok()) { // get horizontal speed - const float speed = inertial_nav.get_speed_xy(); + const float speed = inertial_nav.get_speed_xy_cms(); moving = (speed >= HELI_DYNAMIC_FLIGHT_SPEED_MIN); }else{ // with no GPS lock base it on throttle and forward lean angle diff --git a/ArduCopter/inertia.cpp b/ArduCopter/inertia.cpp index 89ba4abc25..bd9c5bdab0 100644 --- a/ArduCopter/inertia.cpp +++ b/ArduCopter/inertia.cpp @@ -18,7 +18,7 @@ void Copter::read_inertia() } // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin - const int32_t alt_above_origin_cm = inertial_nav.get_altitude(); + const int32_t alt_above_origin_cm = inertial_nav.get_position_z_up_cm(); current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN); if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { // if home has not been set yet we treat alt-above-origin as alt-above-home diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 99fa4c0217..a1c536409e 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -84,7 +84,7 @@ void Copter::update_land_detector() bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar); // check that vertical speed is within 1m/s of zero - bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100 * land_detector_scalar; + bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100 * land_detector_scalar; // if we have a healthy rangefinder only allow landing detection below 2 meters bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 45a25951db..acc71b7c4b 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -569,7 +569,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(); + const Vector2f current_pos = inertial_nav.get_position_xy_cm(); // target is this many cm away from the vehicle target_error_cm = (target_pos - current_pos).length(); } @@ -647,10 +647,10 @@ void Mode::land_run_horizontal_control() if (doing_precision_landing) { Vector2f target_pos, target_vel_rel; if (!copter.precland.get_target_position_cm(target_pos)) { - target_pos = inertial_nav.get_position_xy(); + target_pos = inertial_nav.get_position_xy_cm(); } if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) { - target_vel_rel = -inertial_nav.get_velocity_xy(); + target_vel_rel = -inertial_nav.get_velocity_xy_cms(); } pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y); pos_control->override_vehicle_velocity_xy(-target_vel_rel); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 7933863c2e..44aa0a556e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -355,7 +355,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi // check our distance from edge of circle Vector3f circle_edge_neu; copter.circle_nav->get_closest_point_on_circle(circle_edge_neu); - float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length(); + float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length(); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { @@ -375,7 +375,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().topostype(), copter.circle_nav->get_center().xy()); + const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), copter.circle_nav->get_center().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() != AUTO_YAW_ROI) { @@ -1753,15 +1753,15 @@ bool ModeAuto::verify_payload_place() FALLTHROUGH; case PayloadPlaceStateType_Descending_Start: nav_payload_place.descend_start_timestamp = now; - nav_payload_place.descend_start_altitude = inertial_nav.get_altitude(); + nav_payload_place.descend_start_altitude = inertial_nav.get_position_z_up_cm(); nav_payload_place.descend_throttle_level = 0; nav_payload_place.state = PayloadPlaceStateType_Descending; FALLTHROUGH; case PayloadPlaceStateType_Descending: // make sure we don't descend too far: - debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max); + debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm()), nav_payload_place.descend_max); if (!is_zero(nav_payload_place.descend_max) && - nav_payload_place.descend_start_altitude - inertial_nav.get_altitude() > nav_payload_place.descend_max) { + nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm() > nav_payload_place.descend_max) { nav_payload_place.state = PayloadPlaceStateType_Ascending; gcs().send_text(MAV_SEVERITY_WARNING, "Reached maximum descent"); return false; // we'll do any cleanups required next time through the loop @@ -1819,7 +1819,7 @@ bool ModeAuto::verify_payload_place() } FALLTHROUGH; case PayloadPlaceStateType_Ascending_Start: { - Location target_loc(inertial_nav.get_position(), Location::AltFrame::ABOVE_ORIGIN); + Location target_loc(inertial_nav.get_position_neu_cm(), Location::AltFrame::ABOVE_ORIGIN); target_loc.alt = nav_payload_place.descend_start_altitude; wp_start(target_loc); nav_payload_place.state = PayloadPlaceStateType_Ascending; diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 4e9ff3f4b4..dc8873c14c 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -86,7 +86,7 @@ void ModeAutorotate::run() uint32_t now = millis(); //milliseconds // Initialise internal variables - float curr_vel_z = inertial_nav.get_velocity().z; // Current vertical descent + float curr_vel_z = inertial_nav.get_velocity_z_up_cms(); // Current vertical descent //---------------------------------------------------------------- // State machine logic diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index e7a626194e..29fe7803d0 100644 --- a/ArduCopter/mode_brake.cpp +++ b/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_xy(inertial_nav.get_velocity().length(), BRAKE_MODE_DECEL_RATE); - pos_control->set_correction_speed_accel_xy(inertial_nav.get_velocity().length(), BRAKE_MODE_DECEL_RATE); + pos_control->set_max_speed_accel_xy(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE); + pos_control->set_correction_speed_accel_xy(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE); // initialise position controller pos_control->init_xy_controller(); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 4c68662a5b..b02bff21ac 100644 --- a/ArduCopter/mode_drift.cpp +++ b/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(); + const Vector3f& vel = inertial_nav.get_velocity_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 diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 962921614a..751a374f5f 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/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_altitude() * 0.01; + last_ins_height = copter.inertial_nav.get_position_z_up_cm() * 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_altitude() * 0.01; + float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01; float height_estimate = ins_height + height_offset; // compensate for height, this converts to (approx) m/s @@ -347,7 +347,7 @@ void ModeFlowHold::run() */ void ModeFlowHold::update_height_estimate(void) { - float ins_height = copter.inertial_nav.get_altitude() * 0.01; + float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01; #if 1 // assume on ground when disarmed, or if we have only just started spooling the motors up diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 2cb0f61db1..afa348503b 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -108,7 +108,7 @@ void ModeFollow::run() // calculate vehicle heading switch (g2.follow.get_yaw_behave()) { case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: { - if (dist_vec.xy().length() > 1.0f) { + if (dist_vec.xy().length_squared() > 1.0) { yaw_cd = get_bearing_cd(Vector2f{}, dist_vec.xy()); use_yaw = true; } @@ -125,7 +125,7 @@ void ModeFollow::run() } case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: { - if (desired_velocity_neu_cms.xy().length() > 100.0f) { + if (desired_velocity_neu_cms.xy().length_squared() > (100.0 * 100.0)) { yaw_cd = get_bearing_cd(Vector2f{}, desired_velocity_neu_cms.xy()); use_yaw = true; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 6a558bf2cc..6dd0af4717 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -1060,7 +1060,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(); + guided_limit.start_pos = inertial_nav.get_position_neu_cm(); } // limit_check - returns true if guided mode has breached a limit @@ -1073,7 +1073,7 @@ bool ModeGuided::limit_check() } // get current location - const Vector3f& curr_pos = inertial_nav.get_position(); + const Vector3f& curr_pos = inertial_nav.get_position_neu_cm(); // check if we have gone below min alt if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) { @@ -1118,7 +1118,7 @@ uint32_t ModeGuided::wp_distance() const case SubMode::WP: return wp_nav->get_wp_distance_to_destination(); case SubMode::Pos: - return get_horizontal_distance_cm(inertial_nav.get_position_xy(), guided_pos_target_cm.tofloat().xy()); + return get_horizontal_distance_cm(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_pos_error_xy_cm(); break; @@ -1133,7 +1133,7 @@ int32_t ModeGuided::wp_bearing() const case SubMode::WP: return wp_nav->get_wp_bearing_to_destination(); case SubMode::Pos: - return get_bearing_cd(inertial_nav.get_position_xy(), guided_pos_target_cm.tofloat().xy()); + return get_bearing_cd(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_bearing_to_target_cd(); break; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 0dae994bd9..edb72faa5f 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -61,10 +61,10 @@ void ModeLoiter::precision_loiter_xy() loiter_nav->clear_pilot_desired_acceleration(); Vector2f target_pos, target_vel_rel; if (!copter.precland.get_target_position_cm(target_pos)) { - target_pos = inertial_nav.get_position_xy(); + target_pos = inertial_nav.get_position_xy_cm(); } if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) { - target_vel_rel = -inertial_nav.get_velocity_xy(); + target_vel_rel = -inertial_nav.get_velocity_xy_cms(); } pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y); pos_control->override_vehicle_velocity_xy(-target_vel_rel); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index ee06d7e9bb..4862c12295 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/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(); + const Vector3f& vel = inertial_nav.get_velocity_neu_cms(); // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); @@ -378,7 +378,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(inertial_nav.get_position_xy()); + loiter_nav->init_target(inertial_nav.get_position_xy_cm()); // set delay to start of wind compensation estimate updates wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; } @@ -569,7 +569,7 @@ void ModePosHold::update_wind_comp_estimate() } // check horizontal velocity is low - if (inertial_nav.get_speed_xy() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) { + if (inertial_nav.get_speed_xy_cms() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) { return; } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 6e4bd999fc..df73c4db42 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -71,9 +71,9 @@ void ModeThrow::run() // initialise the demanded height to 3m above the throw height // we want to rapidly clear surrounding obstacles if (g2.throw_type == ThrowType::Drop) { - pos_control->set_pos_target_z_cm(inertial_nav.get_altitude() - 100); + pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - 100); } else { - pos_control->set_pos_target_z_cm(inertial_nav.get_altitude() + 300); + pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 300); } // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum @@ -204,8 +204,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().length(); - const float velocity_z = inertial_nav.get_velocity().z; + const float velocity = inertial_nav.get_velocity_neu_cms().length(); + const float velocity_z = inertial_nav.get_velocity_z_up_cms(); 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(); @@ -255,14 +255,14 @@ bool ModeThrow::throw_detected() } // Check for high speed (>500 cm/s) - bool high_speed = inertial_nav.get_velocity().length() > THROW_HIGH_SPEED; + bool high_speed = inertial_nav.get_velocity_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 < -THROW_VERTICAL_SPEED; + changing_height = inertial_nav.get_velocity_z_up_cms() < -THROW_VERTICAL_SPEED; } else { - changing_height = inertial_nav.get_velocity().z > THROW_VERTICAL_SPEED; + changing_height = inertial_nav.get_velocity_z_up_cms() > THROW_VERTICAL_SPEED; } // Check the vertical acceleraton is greater than 0.25g @@ -277,11 +277,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; + free_fall_start_velz = inertial_nav.get_velocity_z_up_cms(); } // 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 - free_fall_start_velz) < -250.0f)); + 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)); // start motors and enter the control mode if we are in continuous freefall return throw_condition_confirmed; diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 0833358ef5..2bc240069d 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -161,7 +161,7 @@ void ModeZigZag::run() void ModeZigZag::save_or_move_to_destination(Destination ab_dest) { // get current position as an offset from EKF origin - const Vector2f curr_pos {inertial_nav.get_position_xy()}; + const Vector2f curr_pos {inertial_nav.get_position_xy_cm()}; // handle state machine changes switch (stage) { @@ -427,7 +427,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve } // get distance from vehicle to start_pos - const Vector2f curr_pos2d {inertial_nav.get_position_xy()}; + const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()}; Vector2f veh_to_start_pos = curr_pos2d - start_pos; // lengthen AB_diff so that it is at least as long as vehicle is from start point @@ -458,7 +458,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_altitude(); + next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm(); } } @@ -499,7 +499,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con float scalar = constrain_float(_side_dist, 0.1f, 100.0f) * 100 / safe_sqrt(AB_side.length_squared()); // get distance from vehicle to start_pos - const Vector2f curr_pos2d {inertial_nav.get_position_xy()}; + const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()}; next_dest.x = curr_pos2d.x + (AB_side.x * scalar); next_dest.y = curr_pos2d.y + (AB_side.y * scalar); @@ -510,7 +510,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_altitude(); + next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm(); } return true; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 6955e211d5..591fabd20d 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -55,7 +55,7 @@ void Copter::read_rangefinder(void) rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient); // remember inertial alt to allow us to interpolate rangefinder - rf_state.inertial_alt_cm = inertial_nav.get_altitude(); + rf_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm(); // glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading // are considered a glitch and glitch_count becomes non-zero @@ -141,7 +141,7 @@ bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) return false; } ret = rangefinder_state.alt_cm_filt.get(); - float inertial_alt_cm = inertial_nav.get_altitude(); + float inertial_alt_cm = inertial_nav.get_position_z_up_cm(); ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm; return true; } diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 4812903aa4..63f48902ad 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -17,7 +17,7 @@ void Copter::SurfaceTracking::update_surface_offset() // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m. curr_surface_alt_above_origin_cm is 7m (or 700cm) RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f; - float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_altitude() - dir * rf_state.alt_cm; + const float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_position_z_up_cm() - dir * rf_state.alt_cm; // update position controller target offset to the surface's alt above the EKF origin copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm); @@ -74,7 +74,7 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm) if (surface != Surface::GROUND) { return; } - copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_altitude() - _target_alt_cm); + copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_position_z_up_cm() - _target_alt_cm); last_update_ms = AP_HAL::millis(); } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 4483432fe2..687287336b 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -133,7 +133,7 @@ void Mode::auto_takeoff_run() // check if we are not navigating because of low altitude if (auto_takeoff_no_nav_active) { // check if vehicle has reached no_nav_alt threshold - if (inertial_nav.get_altitude() >= auto_takeoff_no_nav_alt_cm) { + if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) { auto_takeoff_no_nav_active = false; wp_nav->shift_wp_origin_and_destination_to_stopping_point_xy(); } else { @@ -173,7 +173,7 @@ void Mode::auto_takeoff_set_start_alt(void) { if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) { // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min - auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude() + g2.wp_navalt_min * 100; + auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; auto_takeoff_no_nav_active = true; } else { auto_takeoff_no_nav_active = false; diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index f333ef7b7a..b80539f62e 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -846,7 +846,7 @@ void ToyMode::throttle_adjust(float &throttle_control) } // limit descent rate close to the ground - float height = copter.inertial_nav.get_altitude() * 0.01 - copter.arming_altitude_m; + float height = copter.inertial_nav.get_position_z_up_cm() * 0.01 - copter.arming_altitude_m; if (throttle_control < 500 && height < TOY_DESCENT_SLOW_HEIGHT + TOY_DESCENT_SLOW_RAMP && copter.motors->armed() && !copter.ap.land_complete) {