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

Copter: INAV rename for neu & cm/cms

This commit is contained in:
Josh Henderson 2021-10-20 04:29:57 -04:00 committed by Andrew Tridgell
parent da418ed520
commit 3107c42fca
26 changed files with 67 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/AP_Arming.cpp
View File

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

2
ArduCopter/Attitude.cpp
View File

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

2
ArduCopter/GCS_Mavlink.cpp
View File

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

4
ArduCopter/Log.cpp
View File

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

8
ArduCopter/autoyaw.cpp
View File

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

8
ArduCopter/baro_ground_effect.cpp
View File

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

4
ArduCopter/crash_check.cpp
View File

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

2
ArduCopter/heli.cpp
View File

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

2
ArduCopter/inertia.cpp
View File

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

2
ArduCopter/land_detector.cpp
View File

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

6
ArduCopter/mode.cpp
View File

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

12
ArduCopter/mode_auto.cpp
View File

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

2
ArduCopter/mode_autorotate.cpp
View File

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

4
ArduCopter/mode_brake.cpp
View File

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

2
ArduCopter/mode_drift.cpp
View File

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

6
ArduCopter/mode_flowhold.cpp
View File

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

4
ArduCopter/mode_follow.cpp
View File

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

8
ArduCopter/mode_guided.cpp
View File

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

4
ArduCopter/mode_loiter.cpp
View File

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

6
ArduCopter/mode_poshold.cpp
View File

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

18
ArduCopter/mode_throw.cpp
View File

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

10
ArduCopter/mode_zigzag.cpp
View File

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

4
ArduCopter/sensors.cpp
View File

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

4
ArduCopter/surface_tracking.cpp
View File

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

4
ArduCopter/takeoff.cpp
View File

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

2
ArduCopter/toy_mode.cpp
View File

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