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

Plane: Small cleanup to quadplane const (and clarity)

This commit is contained in:
Michael du Breuil 2018-08-23 22:42:37 -07:00 committed by Andrew Tridgell
parent e11c7c6069
commit 925d76bb8c
2 changed files with 27 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
ArduPlane/quadplane.cpp
View File

@ -483,7 +483,7 @@ bool QuadPlane::setup(void)
} }
if (hal.util->available_memory() < if (hal.util->available_memory() <
4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav)) { 4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav) + sizeof(*ahrs_view) + sizeof(*loiter_nav)) {
gcs().send_text(MAV_SEVERITY_INFO, "Not enough memory for quadplane"); gcs().send_text(MAV_SEVERITY_INFO, "Not enough memory for quadplane");
goto failed; goto failed;
} }
@ -883,23 +883,25 @@ bool QuadPlane::is_flying(void)
// crude landing detector to prevent tipover // crude landing detector to prevent tipover
bool QuadPlane::should_relax(void) bool QuadPlane::should_relax(void)
{ {
const uint32_t tnow = millis();
bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min(); bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
if (motors->get_throttle() < 0.01f) { if (motors->get_throttle() < 0.01f) {
motor_at_lower_limit = true; motor_at_lower_limit = true;
} }
if (!motor_at_lower_limit) { if (!motor_at_lower_limit) {
landing_detect.lower_limit_start_ms = 0; landing_detect.lower_limit_start_ms = 0;
return false;
} else if (landing_detect.lower_limit_start_ms == 0) {
landing_detect.lower_limit_start_ms = tnow;
} }
if (motor_at_lower_limit && landing_detect.lower_limit_start_ms == 0) {
landing_detect.lower_limit_start_ms = millis(); return (tnow - landing_detect.lower_limit_start_ms) > 1000;
}
bool relax_loiter = landing_detect.lower_limit_start_ms != 0 &&
(millis() - landing_detect.lower_limit_start_ms) > 1000;
return relax_loiter;
} }
// see if we are flying in vtol // see if we are flying in vtol
bool QuadPlane::is_flying_vtol(void) bool QuadPlane::is_flying_vtol(void) const
{ {
if (!available()) { if (!available()) {
return false; return false;
@ -926,7 +928,7 @@ bool QuadPlane::is_flying_vtol(void)
smooth out descent rate for landing to prevent a jerk as we get to smooth out descent rate for landing to prevent a jerk as we get to
land_final_alt. land_final_alt.
*/ */
float QuadPlane::landing_descent_rate_cms(float height_above_ground) float QuadPlane::landing_descent_rate_cms(float height_above_ground) const
{ {
float ret = linear_interpolate(land_speed_cms, wp_nav->get_speed_down(), float ret = linear_interpolate(land_speed_cms, wp_nav->get_speed_down(),
height_above_ground, height_above_ground,
@ -951,10 +953,11 @@ void QuadPlane::control_loiter()
loiter_nav->soften_for_landing(); loiter_nav->soften_for_landing();
} }
if (millis() - last_loiter_ms > 500) { const uint32_t now = AP_HAL::millis();
if (now - last_loiter_ms > 500) {
loiter_nav->init_target(); loiter_nav->init_target();
} }
last_loiter_ms = millis(); last_loiter_ms = now;
// motors use full range // motors use full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -979,7 +982,6 @@ void QuadPlane::control_loiter()
plane.nav_roll_cd = loiter_nav->get_roll(); plane.nav_roll_cd = loiter_nav->get_roll();
plane.nav_pitch_cd = loiter_nav->get_pitch(); plane.nav_pitch_cd = loiter_nav->get_pitch();
const uint32_t now = AP_HAL::millis();
if (now - last_pidz_init_ms < (uint32_t)transition_time_ms*2 && !is_tailsitter()) { if (now - last_pidz_init_ms < (uint32_t)transition_time_ms*2 && !is_tailsitter()) {
// we limit pitch during initial transition // we limit pitch during initial transition
float pitch_limit_cd = linear_interpolate(loiter_initial_pitch_cd, aparm.angle_max, float pitch_limit_cd = linear_interpolate(loiter_initial_pitch_cd, aparm.angle_max,
@ -1021,7 +1023,7 @@ void QuadPlane::control_loiter()
/* /*
get pilot input yaw rate in cd/s get pilot input yaw rate in cd/s
*/ */
float QuadPlane::get_pilot_input_yaw_rate_cds(void) float QuadPlane::get_pilot_input_yaw_rate_cds(void) const
{ {
if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) { if (plane.channel_throttle->get_control_in() <= 0 && !plane.auto_throttle_mode) {
// the user may be trying to disarm // the user may be trying to disarm
@ -1056,7 +1058,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void)
} }
// get pilot desired climb rate in cm/s // get pilot desired climb rate in cm/s
float QuadPlane::get_pilot_desired_climb_rate_cms(void) float QuadPlane::get_pilot_desired_climb_rate_cms(void) const
{ {
if (plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter > 0) { if (plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter > 0) {
// descend at 0.5m/s for now // descend at 0.5m/s for now
@ -1094,7 +1096,7 @@ void QuadPlane::set_armed(bool armed)
/* /*
estimate desired climb rate for assistance (in cm/s) estimate desired climb rate for assistance (in cm/s)
*/ */
float QuadPlane::assist_climb_rate_cms(void) float QuadPlane::assist_climb_rate_cms(void) const
{ {
float climb_rate; float climb_rate;
if (plane.auto_throttle_mode) { if (plane.auto_throttle_mode) {
@ -1119,7 +1121,7 @@ float QuadPlane::assist_climb_rate_cms(void)
/* /*
calculate desired yaw rate for assistance calculate desired yaw rate for assistance
*/ */
float QuadPlane::desired_auto_yaw_rate_cds(void) float QuadPlane::desired_auto_yaw_rate_cds(void) const
{ {
float aspeed; float aspeed;
if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.aparm.airspeed_min) { if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.aparm.airspeed_min) {
@ -2371,12 +2373,13 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
weathervane.last_output = 0; weathervane.last_output = 0;
return 0; return 0;
} }
const uint32_t tnow = millis();
if (plane.channel_rudder->get_control_in() != 0) { if (plane.channel_rudder->get_control_in() != 0) {
weathervane.last_pilot_input_ms = AP_HAL::millis(); weathervane.last_pilot_input_ms = tnow;
weathervane.last_output = 0; weathervane.last_output = 0;
return 0; return 0;
} }
if (AP_HAL::millis() - weathervane.last_pilot_input_ms < 3000) { if (tnow - weathervane.last_pilot_input_ms < 3000) {
weathervane.last_output = 0; weathervane.last_output = 0;
return 0; return 0;
} }

12
ArduPlane/quadplane.h
View File

@ -84,7 +84,7 @@ public:
float get_weathervane_yaw_rate_cds(void); float get_weathervane_yaw_rate_cds(void);
// see if we are flying from vtol point of view // see if we are flying from vtol point of view
bool is_flying_vtol(void); bool is_flying_vtol(void) const;
// return true when tailsitter frame configured // return true when tailsitter frame configured
bool is_tailsitter(void) const; bool is_tailsitter(void) const;
@ -168,13 +168,13 @@ private:
void hold_stabilize(float throttle_in); void hold_stabilize(float throttle_in);
// get pilot desired yaw rate in cd/s // get pilot desired yaw rate in cd/s
float get_pilot_input_yaw_rate_cds(void); float get_pilot_input_yaw_rate_cds(void) const;
// get overall desired yaw rate in cd/s // get overall desired yaw rate in cd/s
float get_desired_yaw_rate_cds(void); float get_desired_yaw_rate_cds(void);
// get desired climb rate in cm/s // get desired climb rate in cm/s
float get_pilot_desired_climb_rate_cms(void); float get_pilot_desired_climb_rate_cms(void) const;
// initialise throttle_wait when entering mode // initialise throttle_wait when entering mode
void init_throttle_wait(); void init_throttle_wait();
@ -198,15 +198,15 @@ private:
void init_qrtl(void); void init_qrtl(void);
void control_qrtl(void); void control_qrtl(void);
float assist_climb_rate_cms(void); float assist_climb_rate_cms(void) const;
// calculate desired yaw rate for assistance // calculate desired yaw rate for assistance
float desired_auto_yaw_rate_cds(void); float desired_auto_yaw_rate_cds(void) const;
bool should_relax(void); bool should_relax(void);
void motors_output(void); void motors_output(void);
void Log_Write_QControl_Tuning(); void Log_Write_QControl_Tuning();
float landing_descent_rate_cms(float height_above_ground); float landing_descent_rate_cms(float height_above_ground) const;
// setup correct aux channels for frame class // setup correct aux channels for frame class
void setup_default_channels(uint8_t num_motors); void setup_default_channels(uint8_t num_motors);