コード例 #1
0
ファイル: control_throw.cpp プロジェクト: 303414326/ardupilot
// runs the throw to start controller
// should be called at 100hz or more
void Copter::throw_run()
{
    static ThrowModeState throw_state = Throw_Disarmed;

    /* Throw State Machine
    Throw_Disarmed - motors are off
    Throw_Detecting -  motors are on and we are waiting for the throw
    Throw_Uprighting - the throw has been detected and the copter is being uprighted
    Throw_HgtStabilise - the copter is kept level and  height is stabilised about the target height
    Throw_PosHold - the copter is kept at a constant position and height
    */

    // Don't enter THROW mode if interlock will prevent motors running
    if (!motors.armed() && motors.get_interlock()) {
        // state machine entry is always from a disarmed state
        throw_state = Throw_Disarmed;

        // remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode before starting motors
        throw_early_exit_interlock = true;

        // prevent motors from rotating before the throw is detected unless enabled by the user
        if (g.throw_motor_start == 1) {
            motors.set_interlock(true);
        } else {
            motors.set_interlock(false);
        }

        // status to let system know flight control has not started which means the interlock setting needs to restored if we exit to another flight mode
        // this is necessary because throw mode uses the interlock to achieve a post arm motor start.
        throw_flight_commenced = false;

    } else if (throw_state == Throw_Disarmed && motors.armed()) {
        gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
        throw_state = Throw_Detecting;

        // prevent motors from rotating before the throw is detected unless enabled by the user
        if (g.throw_motor_start == 1) {
            motors.set_interlock(true);
        } else {
            motors.set_interlock(false);
        }

    } else if (throw_state == Throw_Detecting && throw_detected()){
        gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
        throw_state = Throw_Uprighting;

        // Cancel the waiting for throw tone sequence
        AP_Notify::flags.waiting_for_throw = false;

        // reset the interlock
        motors.set_interlock(true);

        // status to let system know flight control has started which means the entry interlock setting will not restored if we exit to another flight mode
        throw_flight_commenced = true;

    } else if (throw_state == Throw_Uprighting && throw_attitude_good()) {
        gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
        throw_state = Throw_HgtStabilise;

        // initialize vertical speed and acceleration limits
        // use brake mode values for rapid response
        pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
        pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);

        // initialise the demanded height to 3m above the throw height
        // we want to rapidly clear surrounding obstacles
        pos_control.set_alt_target(inertial_nav.get_altitude() + 300);

        // set the initial velocity of the height controller demand to the measured velocity if it is going up
        // if it is going down, set it to zero to enforce a very hard stop
        pos_control.set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        set_auto_armed(true);

    } else if (throw_state == Throw_HgtStabilise && throw_height_good()) {
        gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
        throw_state = Throw_PosHold;

        // initialise the loiter target to the curent position and velocity
        wp_nav.init_loiter_target();

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        set_auto_armed(true);
    }

    // Throw State Processing
    switch (throw_state) {

    case Throw_Disarmed:

        // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
        motors.slow_start(true);

        break;

    case Throw_Detecting:

        // Hold throttle at zero during the throw and continually reset the attitude controller
        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);

        // Play the waiting for throw tone sequence to alert the user
        AP_Notify::flags.waiting_for_throw = true;

        break;

    case Throw_Uprighting:

        // demand a level roll/pitch attitude with zero yaw rate
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);

        // output 50% throttle and turn off angle boost to maximise righting moment
        attitude_control.set_throttle_out(500, false, g.throttle_filt);

        break;

    case Throw_HgtStabilise:

        // call attitude controller
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);

        // call height controller
        pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
        pos_control.update_z_controller();

        break;

    case Throw_PosHold:

        // run loiter controller
        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);

        // call attitude controller
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);

        // call height controller
        pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
        pos_control.update_z_controller();

        break;
    }
}
コード例 #2
0
ファイル: control_throw.cpp プロジェクト: EijiAoki/ardupilot
// runs the throw to start controller
// should be called at 100hz or more
void Copter::throw_run()
{
    /* Throw State Machine
    Throw_Disarmed - motors are off
    Throw_Detecting -  motors are on and we are waiting for the throw
    Throw_Uprighting - the throw has been detected and the copter is being uprighted
    Throw_HgtStabilise - the copter is kept level and  height is stabilised about the target height
    Throw_PosHold - the copter is kept at a constant position and height
    */

    // Don't enter THROW mode if interlock will prevent motors running
    if (!motors.armed() && motors.get_interlock()) {
        // state machine entry is always from a disarmed state
        throw_state.stage = Throw_Disarmed;

    } else if (throw_state.stage == Throw_Disarmed && motors.armed()) {
        gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
        throw_state.stage = Throw_Detecting;

    } else if (throw_state.stage == Throw_Detecting && throw_detected()){
        gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
        throw_state.stage = Throw_Uprighting;

        // Cancel the waiting for throw tone sequence
        AP_Notify::flags.waiting_for_throw = false;

    } else if (throw_state.stage == Throw_Uprighting && throw_attitude_good()) {
        gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
        throw_state.stage = Throw_HgtStabilise;

        // initialize vertical speed and acceleration limits
        // use brake mode values for rapid response
        pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
        pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);

        // initialise the demanded height to 3m above the throw height
        // we want to rapidly clear surrounding obstacles
        if (g2.throw_type == ThrowType_Drop) {
            pos_control.set_alt_target(inertial_nav.get_altitude() - 100);
        } else {
            pos_control.set_alt_target(inertial_nav.get_altitude() + 300);
        }

        // set the initial velocity of the height controller demand to the measured velocity if it is going up
        // if it is going down, set it to zero to enforce a very hard stop
        pos_control.set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        set_auto_armed(true);

    } else if (throw_state.stage == Throw_HgtStabilise && throw_height_good()) {
        gcs_send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
        throw_state.stage = Throw_PosHold;

        // initialise the loiter target to the curent position and velocity
        wp_nav.init_loiter_target();

        // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
        set_auto_armed(true);
    } else if (throw_state.stage == Throw_PosHold && throw_position_good()) {
        if (!throw_state.nextmode_attempted) {
            switch (g2.throw_nextmode) {
                case AUTO:
                case GUIDED:
                case RTL:
                case LAND:
                case BRAKE:
                    set_mode((control_mode_t)g2.throw_nextmode.get(), MODE_REASON_THROW_COMPLETE);
                    break;
                default:
                    // do nothing
                    break;
            }
            throw_state.nextmode_attempted = true;
        }
    }

    // Throw State Processing
    switch (throw_state.stage) {

    case Throw_Disarmed:

        // prevent motors from rotating before the throw is detected unless enabled by the user
        if (g.throw_motor_start == 1) {
            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
        } else {
            motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
        }

        // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
        break;

    case Throw_Detecting:

        // prevent motors from rotating before the throw is detected unless enabled by the user
        if (g.throw_motor_start == 1) {
            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
        } else {
            motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
        }

        // Hold throttle at zero during the throw and continually reset the attitude controller
        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);

        // Play the waiting for throw tone sequence to alert the user
        AP_Notify::flags.waiting_for_throw = true;

        break;

    case Throw_Uprighting:

        // set motors to full range
        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

        // demand a level roll/pitch attitude with zero yaw rate
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());

        // output 50% throttle and turn off angle boost to maximise righting moment
        attitude_control.set_throttle_out(500, false, g.throttle_filt);

        break;

    case Throw_HgtStabilise:

        // set motors to full range
        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

        // call attitude controller
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());

        // call height controller
        pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
        pos_control.update_z_controller();

        break;

    case Throw_PosHold:

        // set motors to full range
        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

        // run loiter controller
        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);

        // call attitude controller
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());

        // call height controller
        pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
        pos_control.update_z_controller();

        break;
    }

    // log at 10hz or if stage changes
    uint32_t now = AP_HAL::millis();
    if ((throw_state.stage != throw_state.prev_stage) || (now - throw_state.last_log_ms) > 100) {
        throw_state.prev_stage = throw_state.stage;
        throw_state.last_log_ms = now;
        float velocity = inertial_nav.get_velocity().length();
        float velocity_z = inertial_nav.get_velocity().z;
        float accel = ins.get_accel().length();
        float ef_accel_z = ahrs.get_accel_ef().z;
        bool throw_detect = (throw_state.stage > Throw_Detecting) || throw_detected();
        bool attitude_ok = (throw_state.stage > Throw_Uprighting) || throw_attitude_good();
        bool height_ok = (throw_state.stage > Throw_HgtStabilise) || throw_height_good();
        bool pos_ok = (throw_state.stage > Throw_PosHold) || throw_position_good();
        Log_Write_Throw(throw_state.stage,
                        velocity,
                        velocity_z,
                        accel,
                        ef_accel_z,
                        throw_detect,
                        attitude_ok,
                        height_ok,
                        pos_ok);
    }
}