Esempio n. 1
0
// read_3pos_switch
RC_Channel::aux_switch_pos_t RC_Channel::read_3pos_switch() const
{
    const uint16_t in = get_radio_in();
    if (in < AUX_PWM_TRIGGER_LOW) return LOW;   // switch is in low position
    if (in > AUX_PWM_TRIGGER_HIGH) return HIGH; // switch is in high position
    return MIDDLE;                              // switch is in middle position
}
Esempio n. 2
0
/// map a function to a servo channel and output it
void
RC_Channel_aux::output_ch(void)
{
    // take care of two corner cases
    switch(function)
    {
    case k_none:                // disabled
        return;
    case k_manual:              // manual
        if (_disable_passthrough) {
            set_radio_out(get_radio_trim());
        } else {
            set_radio_out(get_radio_in());
        }
        break;
    case k_rcin1 ... k_rcin16: // rc pass-thru
        if (_disable_passthrough) {
            set_radio_out(get_radio_trim());
        } else {
            set_radio_out(hal.rcin->read(function-k_rcin1));
        }
        break;
    case k_motor1 ... k_motor8:
        // handled by AP_Motors::rc_write()
        return;
    }
    hal.rcout->write(_ch_out, get_radio_out());
}
Esempio n. 3
0
// read_3pos_switch
bool RC_Channel::read_3pos_switch(RC_Channel::aux_switch_pos_t &ret) const
{
    const uint16_t in = get_radio_in();
    if (in <= 900 or in >= 2200) {
        return false;
    }
    if (in < AUX_PWM_TRIGGER_LOW) {
        ret = LOW;
    } else if (in > AUX_PWM_TRIGGER_HIGH) {
        ret = HIGH;
    } else {
        ret = MIDDLE;
    }
    return true;
}
Esempio n. 4
0
void RC_Channel::read_mode_switch()
{
    // calculate position of flight mode switch
    const uint16_t pulsewidth = get_radio_in();
    if (pulsewidth <= 900 || pulsewidth >= 2200) {
        return;  // This is an error condition
    }

    modeswitch_pos_t position;
    if      (pulsewidth < 1231) position = 0;
    else if (pulsewidth < 1361) position = 1;
    else if (pulsewidth < 1491) position = 2;
    else if (pulsewidth < 1621) position = 3;
    else if (pulsewidth < 1750) position = 4;
    else position = 5;

    if (mode_switch_state.last_position == position) {
        // nothing to do
        return;
    }

    const uint32_t tnow_ms = AP_HAL::millis();
    if (position != mode_switch_state.debounced_position) {
        mode_switch_state.debounced_position = position;
        // store time that switch last moved
        mode_switch_state.last_edge_time_ms = tnow_ms;
        return;
    }

    if (tnow_ms - mode_switch_state.last_edge_time_ms < MODE_SWITCH_DEBOUNCE_TIME_MS) {
        // still in debounce
        return;
    }

    // set flight mode and simple mode setting
    mode_switch_changed(position);

    // set the last switch position.  This marks the
    // transition as complete, even if the mode switch actually
    // failed.  This prevents the vehicle changing modes
    // unexpectedly some time later.
    mode_switch_state.last_position = position;
}