示例#1
0
文件: radio.cpp 项目: 10man/ardupilot
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();

}
示例#2
0
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);
}
示例#3
0
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();
}
示例#4
0
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();
}
示例#5
0
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);
}
示例#6
0
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;
    }
}