Rover: add some comments in radio

This commit is contained in:
Pierre Kancir 2017-06-19 17:47:13 +02:00 committed by Randy Mackay
parent feb8c30af4
commit ef2c2e4afc
1 changed files with 6 additions and 5 deletions

View File

@ -127,16 +127,17 @@ void Rover::rudder_arm_disarm_check()
void Rover::read_radio() void Rover::read_radio()
{ {
if (!hal.rcin->new_input()) { if (!hal.rcin->new_input()) {
// check if we lost RC link
control_failsafe(channel_throttle->get_radio_in()); control_failsafe(channel_throttle->get_radio_in());
return; return;
} }
failsafe.last_valid_rc_ms = AP_HAL::millis(); failsafe.last_valid_rc_ms = AP_HAL::millis();
// read the RC value
RC_Channels::set_pwm_all(); RC_Channels::set_pwm_all();
// check that RC value are valid
control_failsafe(channel_throttle->get_radio_in()); control_failsafe(channel_throttle->get_radio_in());
// copy RC throttle input to throttle output
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
// Check if the throttle value is above 50% and we need to nudge // Check if the throttle value is above 50% and we need to nudge
@ -149,7 +150,7 @@ void Rover::read_radio()
} else { } else {
throttle_nudge = 0; throttle_nudge = 0;
} }
// apply RC skid steer mixing
if (g.skid_steer_in) { if (g.skid_steer_in) {
// convert the two radio_in values from skid steering values // convert the two radio_in values from skid steering values
/* /*
@ -180,7 +181,7 @@ void Rover::read_radio()
channel_steer->set_pwm(steer); channel_steer->set_pwm(steer);
channel_throttle->set_pwm(thr); channel_throttle->set_pwm(thr);
} }
// check if we try to do RC arm/disarm
rudder_arm_disarm_check(); rudder_arm_disarm_check();
} }