Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: INAV rename for neu & cm/cms

This commit is contained in:
Josh Henderson 2021-10-20 04:30:56 -04:00 committed by Andrew Tridgell
parent 52adda7c4c
commit bdac4a2416
8 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduSub/Attitude.cpp
View File

@ -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();

2
ArduSub/GCS_Mavlink.cpp
View File

@ -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);

2
ArduSub/Log.cpp
View File

@ -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,

4
ArduSub/commands_logic.cpp
View File

@ -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

6
ArduSub/control_althold.cpp
View File

@ -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();
}

4
ArduSub/control_auto.cpp
View File

@ -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 {

4
ArduSub/control_guided.cpp
View File

@ -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)) {

4
ArduSub/inertia.cpp
View File

@ -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();
}