aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: INAV rename for neu & cm/cms

Browse Source
This commit is contained in:
Josh Henderson 2021-10-20 04:30:34 -04:00 committed by Andrew Tridgell
parent 3107c42fca
commit 52adda7c4c
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduPlane/quadplane.cpp
View File

@ -1151,7 +1151,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const
bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active();
if (!manual_air_mode &&
!is_positive(plane.get_throttle_input()) && !plane.control_mode->does_auto_throttle() &&
plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && !(inertial_nav.get_velocity_z() < -0.5 * get_pilot_velocity_z_max_dn())) {
plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && !(inertial_nav.get_velocity_z_up_cms() < -0.5 * get_pilot_velocity_z_max_dn())) {
// the user may be trying to disarm
return 0;
}
@ -1192,7 +1192,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void)
yaw_cds += desired_auto_yaw_rate_cds();
}
bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active();
if (!is_positive(plane.get_throttle_input()) && !plane.control_mode->does_auto_throttle() && !manual_air_mode && !(inertial_nav.get_velocity_z() < -0.5 * get_pilot_velocity_z_max_dn())) {
if (!is_positive(plane.get_throttle_input()) && !plane.control_mode->does_auto_throttle() && !manual_air_mode && !(inertial_nav.get_velocity_z_up_cms() < -0.5 * get_pilot_velocity_z_max_dn())) {
// the user may be trying to disarm
return 0;
}
@ -1735,7 +1735,7 @@ void QuadPlane::update_throttle_suppression(void)
}
// if our vertical velocity is greater than 1m/s then allow motors to run
if (fabsf(inertial_nav.get_velocity_z()) > 100) {
if (fabsf(inertial_nav.get_velocity_z_up_cms()) > 100) {
return;
}
@ -1790,7 +1790,7 @@ void QuadPlane::update_throttle_hover()
float aspeed;
// calc average throttle if we are in a level hover and low airspeed
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_view->roll_sensor) < 500 && labs(ahrs_view->pitch_sensor) < 500 &&
ahrs.airspeed_estimate(aspeed) && aspeed < plane.aparm.airspeed_min*0.3) {
// Can we set the time constant automatically
@ -2331,7 +2331,7 @@ void QuadPlane::vtol_position_controller(void)
// reset position controller xy target to current position
// because we only want velocity control (no position control)
const Vector2f& curr_pos = inertial_nav.get_position_xy();
const Vector2f& curr_pos = inertial_nav.get_position_xy_cm();
pos_control->set_pos_target_xy_cm(curr_pos.x, curr_pos.y);
pos_control->set_accel_desired_xy_cmss(Vector2f());
@ -2767,7 +2767,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
const float d_total = (plane.next_WP_loc.alt - plane.current_loc.alt) * 0.01f;
const float accel_m_s_s = MAX(10, pilot_accel_z) * 0.01f;
const float vel_max = MAX(10, pilot_velocity_z_max_up) * 0.01f;
const float vel_z = inertial_nav.get_velocity_z() * 0.01f;
const float vel_z = inertial_nav.get_velocity_z_up_cms() * 0.01f;
const float t_accel = (vel_max - vel_z) / accel_m_s_s;
const float d_accel = vel_z * t_accel + 0.5f * accel_m_s_s * sq(t_accel);
const float d_remaining = d_total - d_accel;
@ -2884,7 +2884,7 @@ bool QuadPlane::land_detector(uint32_t timeout_ms)
landing_detect.land_start_ms = 0;
return false;
}
float height = inertial_nav.get_altitude()*0.01f;
float height = inertial_nav.get_position_z_up_cm() * 0.01;
if (landing_detect.land_start_ms == 0) {
landing_detect.land_start_ms = now;
landing_detect.vpos_start_m = height;
@ -2972,7 +2972,7 @@ bool QuadPlane::verify_vtol_land(void)
if (poscontrol.pilot_correction_done) {
reached_position = !poscontrol.pilot_correction_active;
} else {
const float dist = (inertial_nav.get_position().topostype() - poscontrol.target_cm).xy().length() * 0.01;
const float dist = (inertial_nav.get_position_neu_cm().topostype() - poscontrol.target_cm).xy().length() * 0.01;
reached_position = dist < descend_dist_threshold;
}
if (reached_position &&
@ -3035,10 +3035,10 @@ void QuadPlane::Log_Write_QControl_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 : int32_t(plane.barometer.get_altitude() * 100),
target_climb_rate : target_climb_rate_cms,
climb_rate : int16_t(inertial_nav.get_velocity_z()),
climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()),
throttle_mix : attitude_control->get_throttle_mix(),
speed_scaler : tailsitter.log_spd_scaler,
transition_state : transition->get_log_transision_state(),
@ -3300,7 +3300,7 @@ bool QuadPlane::guided_mode_enabled(void)
*/
void QuadPlane::set_alt_target_current(void)
{
pos_control->set_pos_target_z_cm(inertial_nav.get_altitude());
pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm());
}
// user initiated takeoff for guided mode