From bdac4a2416d1845f9648cce7c263cf96355a85c0 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Wed, 20 Oct 2021 04:30:56 -0400 Subject: [PATCH] Sub: INAV rename for neu & cm/cms --- ArduSub/Attitude.cpp | 10 +++++----- ArduSub/GCS_Mavlink.cpp | 2 +- ArduSub/Log.cpp | 2 +- ArduSub/commands_logic.cpp | 4 ++-- ArduSub/control_althold.cpp | 6 +++--- ArduSub/control_auto.cpp | 4 ++-- ArduSub/control_guided.cpp | 4 ++-- ArduSub/inertia.cpp | 4 ++-- 8 files changed, 18 insertions(+), 18 deletions(-) diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 071e9fc5b8..c5dbc67e73 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -64,7 +64,7 @@ float Sub::get_roi_yaw() roi_yaw_counter++; if (roi_yaw_counter >= 4) { roi_yaw_counter = 0; - yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position_xy(), roi_WP.xy()); + yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position_xy_cm(), roi_WP.xy()); } return yaw_look_at_WP_bearing; @@ -72,10 +72,10 @@ float Sub::get_roi_yaw() float Sub::get_look_ahead_yaw() { - const Vector3f& vel = inertial_nav.get_velocity(); - const float speed = vel.xy().length(); + const Vector3f& vel = inertial_nav.get_velocity_neu_cms(); + const float speed_sq = vel.xy().length_squared(); // Commanded Yaw to automatically look ahead. - if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) { + if (position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) { yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f; } return yaw_look_ahead_bearing; @@ -131,7 +131,7 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al static uint32_t last_call_ms = 0; float distance_error; float velocity_correction; - float current_alt = inertial_nav.get_altitude(); + float current_alt = inertial_nav.get_position_z_up_cm(); uint32_t now = AP_HAL::millis(); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index d2eb8057cb..5a91068b59 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -623,7 +623,7 @@ void GCS_MAVLINK_Sub::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 += sub.inertial_nav.get_position(); + pos_vector += sub.inertial_nav.get_position_neu_cm(); } else { // convert from alt-above-home to alt-above-ekf-origin pos_vector.z = sub.pv_alt_above_origin(pos_vector.z); diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 906e9f3225..a4ea5b29a5 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -39,7 +39,7 @@ void Sub::Log_Write_Control_Tuning() throttle_out : motors.get_throttle(), throttle_hover : motors.get_throttle_hover(), desired_alt : pos_control.get_pos_target_z_cm() / 100.0f, - inav_alt : inertial_nav.get_altitude() / 100.0f, + inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), desired_rangefinder_alt : (int16_t)target_rangefinder_alt, rangefinder_alt : rangefinder_state.alt_cm, diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 84fa971b09..25ced66be1 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -513,12 +513,12 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd) // set target altitude if not provided if (is_zero(circle_center.z)) { - circle_center.z = inertial_nav.get_altitude(); + circle_center.z = inertial_nav.get_position_z_up_cm(); } // set lat/lon position if not provided if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { - circle_center.xy() = inertial_nav.get_position_xy(); + circle_center.xy() = inertial_nav.get_position_xy_cm(); } // start circling diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 43770f38a0..2e49e3c046 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -111,19 +111,19 @@ void Sub::control_depth() { // reset z targets to current values pos_control.relax_z_controller(channel_throttle->norm_input()); engageStopZ = true; - lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z()); + lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z_up_cms()); } else { // hold z if (ap.at_bottom) { pos_control.init_z_controller(); - pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom + pos_control.set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 10.0); // set target to 10 cm above bottom } // Detects a zero derivative // When detected, move the altitude set point to the actual position // This will avoid any problem related to joystick delays // or smaller input signals - if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) { + if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z_up_cms()))) { engageStopZ = false; pos_control.init_z_controller(); } diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 422f7a974f..2494b24eb1 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -186,7 +186,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi // check our distance from edge of circle Vector3f circle_edge_neu; 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) { @@ -206,7 +206,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi } // if we are outside the circle, point at the edge, otherwise hold yaw - float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy().topostype(), circle_nav.get_center().xy()); + float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), circle_nav.get_center().xy()); if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } else { diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 7d8c2ae7e3..bcab8732d9 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -524,7 +524,7 @@ void Sub::guided_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(); } // guided_limit_check - returns true if guided mode has breached a limit @@ -537,7 +537,7 @@ bool Sub::guided_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)) { diff --git a/ArduSub/inertia.cpp b/ArduSub/inertia.cpp index b6182c6512..8fc70f85ca 100644 --- a/ArduSub/inertia.cpp +++ b/ArduSub/inertia.cpp @@ -17,9 +17,9 @@ void Sub::read_inertia() return; } - current_loc.alt = inertial_nav.get_altitude(); + current_loc.alt = inertial_nav.get_position_z_up_cm(); // get velocity, altitude is always absolute frame, referenced from // water's surface - climb_rate = inertial_nav.get_velocity_z(); + climb_rate = inertial_nav.get_velocity_z_up_cms(); }