mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Sub: Simplify some returns
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
parent
e229d46612
commit
90601c80e9
@ -199,7 +199,6 @@ uint16_t Sub::get_pilot_speed_dn()
|
|||||||
{
|
{
|
||||||
if (g.pilot_speed_dn == 0) {
|
if (g.pilot_speed_dn == 0) {
|
||||||
return abs(g.pilot_speed_up);
|
return abs(g.pilot_speed_up);
|
||||||
} else {
|
|
||||||
return abs(g.pilot_speed_dn);
|
|
||||||
}
|
}
|
||||||
|
return abs(g.pilot_speed_dn);
|
||||||
}
|
}
|
||||||
|
@ -105,10 +105,5 @@ bool Sub::far_from_EKF_origin(const Location& loc)
|
|||||||
{
|
{
|
||||||
// check distance to EKF origin
|
// check distance to EKF origin
|
||||||
const struct Location &ekf_origin = inertial_nav.get_origin();
|
const struct Location &ekf_origin = inertial_nav.get_origin();
|
||||||
if (get_distance(ekf_origin, loc) > EKF_ORIGIN_MAX_DIST_M) {
|
return (get_distance(ekf_origin, loc) > EKF_ORIGIN_MAX_DIST_M);
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// close enough to origin
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
|
@ -66,11 +66,10 @@ void Sub::althold_run()
|
|||||||
|
|
||||||
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
|
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
|
||||||
return;
|
return;
|
||||||
|
|
||||||
} else {
|
|
||||||
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
|
||||||
|
|
||||||
// get pilot's desired yaw rate
|
// get pilot's desired yaw rate
|
||||||
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||||
|
|
||||||
|
@ -194,10 +194,11 @@ bool Sub::mode_has_manual_throttle(control_mode_t mode)
|
|||||||
// arming_from_gcs should be set to true if the arming request comes from the ground station
|
// arming_from_gcs should be set to true if the arming request comes from the ground station
|
||||||
bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs)
|
bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs)
|
||||||
{
|
{
|
||||||
if (mode_has_manual_throttle(mode) || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
|
return (mode_has_manual_throttle(mode)
|
||||||
return true;
|
|| mode == ALT_HOLD
|
||||||
}
|
|| mode == POSHOLD
|
||||||
return false;
|
|| (arming_from_gcs&& mode == GUIDED)
|
||||||
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device
|
// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device
|
||||||
|
@ -259,10 +259,10 @@ bool Sub::ekf_position_ok()
|
|||||||
// if disarmed we accept a predicted horizontal position
|
// if disarmed we accept a predicted horizontal position
|
||||||
if (!motors.armed()) {
|
if (!motors.armed()) {
|
||||||
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
|
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
|
||||||
} else {
|
|
||||||
// once armed we require a good absolute position and EKF must not be in const_pos_mode
|
|
||||||
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// once armed we require a good absolute position and EKF must not be in const_pos_mode
|
||||||
|
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
|
||||||
}
|
}
|
||||||
|
|
||||||
// optflow_position_ok - returns true if optical flow based position estimate is ok
|
// optflow_position_ok - returns true if optical flow based position estimate is ok
|
||||||
@ -282,9 +282,8 @@ bool Sub::optflow_position_ok()
|
|||||||
// if disarmed we accept a predicted horizontal relative position
|
// if disarmed we accept a predicted horizontal relative position
|
||||||
if (!motors.armed()) {
|
if (!motors.armed()) {
|
||||||
return (filt_status.flags.pred_horiz_pos_rel);
|
return (filt_status.flags.pred_horiz_pos_rel);
|
||||||
} else {
|
|
||||||
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
|
|
||||||
}
|
}
|
||||||
|
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user