mirror of https://github.com/ArduPilot/ardupilot
Copter: fixed RC failsafe handling for no RC receiver
this stops us using uninitialised values in modes like circle which can operate either with or without RC input. If we didn't have a RC receiver attached then they would use a maximum yaw rate (which produces quite a spectacular result for a tuned up racing quad)
This commit is contained in:
parent
e50d237f3e
commit
328fff8585
|
@ -4,6 +4,10 @@
|
||||||
// returns desired yaw rate in centi-degrees per second
|
// returns desired yaw rate in centi-degrees per second
|
||||||
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
|
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
|
||||||
{
|
{
|
||||||
|
// throttle failsafe check
|
||||||
|
if (failsafe.radio || !ap.rc_receiver_present) {
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
float yaw_request;
|
float yaw_request;
|
||||||
|
|
||||||
// calculate yaw rate request
|
// calculate yaw rate request
|
||||||
|
@ -76,7 +80,7 @@ void Copter::set_throttle_takeoff()
|
||||||
float Copter::get_pilot_desired_climb_rate(float throttle_control)
|
float Copter::get_pilot_desired_climb_rate(float throttle_control)
|
||||||
{
|
{
|
||||||
// throttle failsafe check
|
// throttle failsafe check
|
||||||
if (failsafe.radio) {
|
if (failsafe.radio || !ap.rc_receiver_present) {
|
||||||
return 0.0f;
|
return 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -351,6 +351,12 @@ void Mode::update_navigation()
|
||||||
// returns desired angle in centi-degrees
|
// returns desired angle in centi-degrees
|
||||||
void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
|
void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
|
||||||
{
|
{
|
||||||
|
// throttle failsafe check
|
||||||
|
if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
|
||||||
|
roll_out = 0;
|
||||||
|
pitch_out = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
// fetch roll and pitch inputs
|
// fetch roll and pitch inputs
|
||||||
roll_out = channel_roll->get_control_in();
|
roll_out = channel_roll->get_control_in();
|
||||||
pitch_out = channel_pitch->get_control_in();
|
pitch_out = channel_pitch->get_control_in();
|
||||||
|
|
|
@ -94,11 +94,8 @@ void Copter::read_radio()
|
||||||
set_throttle_and_failsafe(channel_throttle->get_radio_in());
|
set_throttle_and_failsafe(channel_throttle->get_radio_in());
|
||||||
set_throttle_zero_flag(channel_throttle->get_control_in());
|
set_throttle_zero_flag(channel_throttle->get_control_in());
|
||||||
|
|
||||||
if (!ap.rc_receiver_present) {
|
// RC receiver must be attached if we've just got input
|
||||||
// RC receiver must be attached if we've just got input and
|
ap.rc_receiver_present = true;
|
||||||
// there are no overrides
|
|
||||||
ap.rc_receiver_present = !RC_Channels::has_active_overrides();
|
|
||||||
}
|
|
||||||
|
|
||||||
// pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
|
// pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
|
||||||
radio_passthrough_to_motors();
|
radio_passthrough_to_motors();
|
||||||
|
|
Loading…
Reference in New Issue