void Rover::read_radio()
{
if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->get_radio_in());
return;
}
failsafe.last_valid_rc_ms = AP_HAL::millis();
RC_Channel::set_pwm_all();
control_failsafe(channel_throttle->get_radio_in());
channel_throttle->set_servo_out(channel_throttle->get_control_in());
// Check if the throttle value is above 50% and we need to nudge
// Make sure its above 50% in the direction we are travelling
if ((abs(channel_throttle->get_servo_out()) > 50) &&
(((channel_throttle->get_servo_out() < 0) && in_reverse) ||
((channel_throttle->get_servo_out() > 0) && !in_reverse))) {
throttle_nudge = (g.throttle_max - g.throttle_cruise) *
((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f);
} else {
throttle_nudge = 0;
}
if (g.skid_steer_in) {
// convert the two radio_in values from skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float motor1 = channel_steer->norm_input();
float motor2 = channel_throttle->norm_input();
float steering_scaled = motor1 - motor2;
float throttle_scaled = 0.5f*(motor1 + motor2);
int16_t steer = channel_steer->get_radio_trim();
int16_t thr = channel_throttle->get_radio_trim();
if (steering_scaled > 0.0f) {
steer += steering_scaled*(channel_steer->get_radio_max()-channel_steer->get_radio_trim());
} else {
steer += steering_scaled*(channel_steer->get_radio_trim()-channel_steer->get_radio_min());
}
if (throttle_scaled > 0.0f) {
thr += throttle_scaled*(channel_throttle->get_radio_max()-channel_throttle->get_radio_trim());
} else {
thr += throttle_scaled*(channel_throttle->get_radio_trim()-channel_throttle->get_radio_min());
}
channel_steer->set_pwm(steer);
channel_throttle->set_pwm(thr);
}
rudder_arm_disarm_check();
}
void Plane::read_radio()
{
if (!RC_Channels::read_input()) {
control_failsafe();
return;
}
if(!failsafe.rc_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
failsafe.last_valid_rc_ms = millis();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->recompute_pwm_no_deadzone();
channel_pitch->recompute_pwm_no_deadzone();
channel_throttle->recompute_pwm_no_deadzone();
channel_rudder->recompute_pwm_no_deadzone();
}
control_failsafe();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
if (g.throttle_nudge && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 50 && geofence_stickmixing()) {
float nudge = (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_disarm_check();
if (g.rudder_only != 0) {
// in rudder only mode we discard rudder input and get target
// attitude from the roll channel.
rudder_input = 0;
} else if (stick_mixing_enabled()) {
rudder_input = channel_rudder->get_control_in();
} else {
// no stick mixing
rudder_input = 0;
}
// potentially swap inputs for tailsitters
quadplane.tailsitter_check_input();
// check for transmitter tuning changes
tuning.check_input(control_mode);
}
void Rover::read_radio()
{
if (!hal.rcin->new_input()) {
// check if we lost RC link
control_failsafe(channel_throttle->get_radio_in());
return;
}
failsafe.last_valid_rc_ms = AP_HAL::millis();
// read the RC value
RC_Channels::set_pwm_all();
// check that RC value are valid
control_failsafe(channel_throttle->get_radio_in());
// apply RC skid steer mixing
if (g.skid_steer_in) {
// convert the two radio_in values from skid steering values
/*
mixing rule:
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
const float left_input = channel_steer->norm_input();
const float right_input = channel_throttle->norm_input();
const float throttle_scaled = 0.5f * (left_input + right_input);
float steering_scaled = constrain_float(left_input - right_input, -1.0f, 1.0f);
// flip the steering direction if requesting the vehicle reverse (to be consistent with separate steering-throttle frames)
if (is_negative(throttle_scaled)) {
steering_scaled = -steering_scaled;
}
int16_t steer = channel_steer->get_radio_trim();
int16_t thr = channel_throttle->get_radio_trim();
if (steering_scaled > 0.0f) {
steer += steering_scaled * (channel_steer->get_radio_max() - channel_steer->get_radio_trim());
} else {
steer += steering_scaled * (channel_steer->get_radio_trim() - channel_steer->get_radio_min());
}
if (throttle_scaled > 0.0f) {
thr += throttle_scaled * (channel_throttle->get_radio_max() - channel_throttle->get_radio_trim());
} else {
thr += throttle_scaled * (channel_throttle->get_radio_trim() - channel_throttle->get_radio_min());
}
channel_steer->set_pwm(steer);
channel_throttle->set_pwm(thr);
}
// check if we try to do RC arm/disarm
rudder_arm_disarm_check();
}
void Rover::read_radio()
{
if (!RC_Channels::read_input()) {
// check if we lost RC link
control_failsafe(channel_throttle->get_radio_in());
return;
}
failsafe.last_valid_rc_ms = AP_HAL::millis();
// check that RC value are valid
control_failsafe(channel_throttle->get_radio_in());
// check if we try to do RC arm/disarm
rudder_arm_disarm_check();
}
void Plane::read_radio()
{
if (!rc().read_input()) {
control_failsafe();
return;
}
if(!failsafe.rc_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
failsafe.last_valid_rc_ms = millis();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->recompute_pwm_no_deadzone();
channel_pitch->recompute_pwm_no_deadzone();
channel_throttle->recompute_pwm_no_deadzone();
channel_rudder->recompute_pwm_no_deadzone();
}
control_failsafe();
if (g.throttle_nudge && channel_throttle->get_control_in() > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->get_control_in() - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_disarm_check();
// potentially swap inputs for tailsitters
quadplane.tailsitter_check_input();
// check for transmitter tuning changes
tuning.check_input(control_mode);
}
void Plane::read_radio()
{
if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->radio_in);
return;
}
failsafe.last_valid_rc_ms = millis();
elevon.ch1_temp = channel_roll->read();
elevon.ch2_temp = channel_pitch->read();
uint16_t pwm_roll, pwm_pitch;
if (g.mix_mode == 0) {
pwm_roll = elevon.ch1_temp;
pwm_pitch = elevon.ch2_temp;
}else{
pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
}
RC_Channel::set_pwm_all();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->set_pwm_no_deadzone(pwm_roll);
channel_pitch->set_pwm_no_deadzone(pwm_pitch);
channel_throttle->set_pwm_no_deadzone(channel_throttle->read());
channel_rudder->set_pwm_no_deadzone(channel_rudder->read());
} else {
channel_roll->set_pwm(pwm_roll);
channel_pitch->set_pwm(pwm_pitch);
}
control_failsafe(channel_throttle->radio_in);
channel_throttle->servo_out = channel_throttle->control_in;
if (g.throttle_nudge && channel_throttle->servo_out > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->servo_out - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_disarm_check();
if (g.rudder_only != 0) {
// in rudder only mode we discard rudder input and get target
// attitude from the roll channel.
rudder_input = 0;
} else {
rudder_input = channel_rudder->control_in;
}
}
