Esempio n. 1
0
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output (in the range 0 to 1) before take-off
// used only for althold, loiter, hybrid flight modes
float Copter::get_throttle_pre_takeoff(float input_thr)
{
    // exit immediately if input_thr is zero
    if (input_thr <= 0.0f) {
        return 0.0f;
    }

    // ensure reasonable throttle values
    input_thr = constrain_float(input_thr,0.0f,1000.0f);

    float in_min = 0.0f;
    float in_max = get_takeoff_trigger_throttle();

    float out_min = 0.0f;
    float out_max = get_non_takeoff_throttle();

    if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
        in_min = channel_throttle->get_control_mid();
    }

    float input_range = in_max-in_min;
    float output_range = out_max-out_min;

    // sanity check ranges
    if (input_range <= 0.0f || output_range <= 0.0f) {
        return 0.0f;
    }

    return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max);
}
Esempio n. 2
0
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Copter::get_throttle_pre_takeoff(float input_thr)
{
    // exit immediately if input_thr is zero
    if (input_thr <= 0.0f) {
        return 0.0f;
    }

    // TODO: does this parameter sanity check really belong here?
    g.throttle_mid = constrain_int16(g.throttle_mid,300,700);

    float in_min = g.throttle_min;
    float in_max = get_takeoff_trigger_throttle();

    float out_min = motors.get_throttle_warn();
    float out_max = get_non_takeoff_throttle();

    if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
        in_min = channel_throttle->get_control_mid();
    }

    float input_range = in_max-in_min;
    float output_range = out_max-out_min;

    // sanity check ranges
    if (input_range <= 0.0f || output_range <= 0.0f) {
        return 0.0f;
    }

    return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max);
}
Esempio n. 3
0
// stabilize_init - initialise stabilize controller
bool Copter::ModeStabilize::init(bool ignore_checks)
{
    // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
    if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
            (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
        return false;
    }

    return true;
}
Esempio n. 4
0
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
// used only for althold, loiter, hybrid flight modes
// returns throttle output 0 to 1000
float Copter::get_throttle_pre_takeoff(float input_thr)
{
    // exit immediately if input_thr is zero
    if (input_thr <= 0.0f) {
        return 0.0f;
    }

    // TODO: does this parameter sanity check really belong here?
    g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);

    float in_min = g.throttle_min;
    float in_max = get_takeoff_trigger_throttle();

#if FRAME_CONFIG == HELI_FRAME
    // helicopters swash will move from bottom to 1/2 of mid throttle
    float out_min = 0;
#else
    // multicopters will output between spin-when-armed and 1/2 of mid throttle
    float out_min = motors.get_throttle_warn();
#endif
    float out_max = get_non_takeoff_throttle();

    if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
        in_min = channel_throttle->get_control_mid();
    }

    float input_range = in_max-in_min;
    float output_range = out_max-out_min;

    // sanity check ranges
    if (input_range <= 0.0f || output_range <= 0.0f) {
        return 0.0f;
    }

    return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max);
}
Esempio n. 5
0
// acro_init - initialise acro controller
bool Sub::acro_init(bool ignore_checks)
{
   // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
   if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) {
       return false;
   }
   // set target altitude to zero for reporting
   pos_control.set_alt_target(0);

   return true;
}
Esempio n. 6
0
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void Copter::update_land_detector()
{
    // land detector can not use the following sensors because they are unreliable during landing
    // barometer altitude :                 ground effect can cause errors larger than 4m
    // EKF vertical velocity or altitude :  poor barometer and large acceleration from ground impact
    // earth frame angle or angle error :   landing on an uneven surface will force the airframe to match the ground angle
    // gyro output :                        on uneven surface the airframe may rock back an forth after landing
    // range finder :                       tend to be problematic at very short distances
    // input throttle :                     in slow land the input throttle may be only slightly less than hover

    if (!motors.armed()) {
        // if disarmed, always landed.
        set_land_complete(true);
    } else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
        // if rotor speed and collective pitch are high then clear landing flag
        if (motors.get_throttle() > get_non_takeoff_throttle() && motors.rotor_runup_complete()) {
#else
        // if throttle output is high then clear landing flag
        if (motors.get_throttle() > get_non_takeoff_throttle()) {
#endif
            set_land_complete(false);
        }
    } else {

#if FRAME_CONFIG == HELI_FRAME
        // check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
        bool motor_at_lower_limit = motors.limit.throttle_lower;
#else
        // check that the average throttle output is near minimum (less than 12.5% hover throttle)
        bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min();
#endif

        // check that the airframe is not accelerating (not falling or breaking after fast forward flight)
        bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);

        if (motor_at_lower_limit && accel_stationary) {
            // landed criteria met - increment the counter and check if we've triggered
            if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
                land_detector_count++;
            } else {
                set_land_complete(true);
            }
        } else {
            // we've sensed movement up or down so reset land_detector
            land_detector_count = 0;
        }
    }

    set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE));
}

void Copter::set_land_complete(bool b)
{
    // if no change, exit immediately
    if( ap.land_complete == b )
        return;

    land_detector_count = 0;

    if(b){
        Log_Write_Event(DATA_LAND_COMPLETE);
    } else {
        Log_Write_Event(DATA_NOT_LANDED);
    }
    ap.land_complete = b;
}

// set land complete maybe flag
void Copter::set_land_complete_maybe(bool b)
{
    // if no change, exit immediately
    if (ap.land_complete_maybe == b)
        return;

    if (b) {
        Log_Write_Event(DATA_LAND_COMPLETE_MAYBE);
    }
    ap.land_complete_maybe = b;
}

// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
//  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
//  has no effect when throttle is above hover throttle
void Copter::update_throttle_thr_mix()
{
#if FRAME_CONFIG != HELI_FRAME
    // if disarmed or landed prioritise throttle
    if(!motors.armed() || ap.land_complete) {
        motors.set_throttle_mix_min();
        return;
    }

    if (mode_has_manual_throttle(control_mode)) {
        // manual throttle
        if(channel_throttle->get_control_in() <= 0) {
            motors.set_throttle_mix_min();
        } else {
            motors.set_throttle_mix_mid();
        }
    } else {
        // autopilot controlled throttle

        // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
        const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
        bool large_angle_request = (norm(angle_target.x, angle_target.y) > 1500.0f);

        // check for large external disturbance - angle error over 30 degrees
        const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
        bool large_angle_error = (norm(angle_error.x, angle_error.y) > 3000.0f);

        // check for large acceleration - falling or high turbulence
        Vector3f accel_ef = ahrs.get_accel_ef_blended();
        accel_ef.z += GRAVITY_MSS;
        bool accel_moving = (accel_ef.length() > 3.0f);

        // check for requested decent
        bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;

        if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
            motors.set_throttle_mix_max();
        } else {
            motors.set_throttle_mix_min();
        }
    }
#endif
}
Esempio n. 7
0
// acro_init - initialise acro controller
bool Copter::acro_init(bool ignore_checks)
{
   // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
   if (motors->armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
           (get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid) > get_non_takeoff_throttle())) {
       return false;
   }
   // set target altitude to zero for reporting
   pos_control->set_alt_target(0);

   return true;
}