Ejemplo n.º 1
0
float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
{
	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();

	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;

	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
	float k_i_rate = _k_i * _tc;

	/* input conditioning */
	if (!isfinite(airspeed)) {
		/* airspeed is NaN, +- INF or not available, pick center of band */
		airspeed = 0.5f * (airspeed_min + airspeed_max);
	} else if (airspeed < airspeed_min) {
		airspeed = airspeed_min;
	}

	float roll_error = roll_setpoint - roll;
	_rate_setpoint = roll_error / _tc;

	/* limit the rate */
	if (_max_rate > 0.01f) {
		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
	}

	_rate_error = _rate_setpoint - roll_rate;


	float ilimit_scaled = 0.0f;

	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {

		float id = _rate_error * k_i_rate * dt * scaler;

		/*
		 * anti-windup: do not allow integrator to increase into the
		 * wrong direction if actuator is at limit
		 */
		if (_last_output < -_max_deflection_rad) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);
		} else if (_last_output > _max_deflection_rad) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
	/* store non-limited output */
	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;

	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
}
Ejemplo n.º 2
0
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
    /* Do not calculate control signal with bad inputs */
    if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
          PX4_ISFINITE(ctl_data.groundspeed) &&
          PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
        return math::constrain(_last_output, -1.0f, 1.0f);
    }

    /* get the usual dt estimate */
    uint64_t dt_micros = ecl_elapsed_time(&_last_run);
    _last_run = ecl_absolute_time();
    float dt = (float)dt_micros * 1e-6f;

    /* lock integral for long intervals */
    bool lock_integrator = ctl_data.lock_integrator;

    if (dt_micros > 500000) {
        lock_integrator = true;
    }

    /* input conditioning */
    float min_speed = 1.0f;

    /* Calculate body angular rate error */
    _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error

    if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {

        float id = _rate_error * dt * ctl_data.groundspeed_scaler;

        /*
         * anti-windup: do not allow integrator to increase if actuator is at limit
         */
        if (_last_output < -1.0f) {
            /* only allow motion to center: increase value */
            id = math::max(id, 0.0f);

        } else if (_last_output > 1.0f) {
            /* only allow motion to center: decrease value */
            id = math::min(id, 0.0f);
        }

        _integrator += id * _k_i;
    }

    /* integrator limit */
    //xxx: until start detection is available: integral part in control signal is limited here
    float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);

    /* Apply PI rate controller and store non-limited output */
    _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
            _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
            integrator_constrained;
    /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
            (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/


    return math::constrain(_last_output, -1.0f, 1.0f);
}
Ejemplo n.º 3
0
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(ISFINITE(ctl_data.roll) &&
	      ISFINITE(ctl_data.pitch) &&
	      ISFINITE(ctl_data.body_y_rate) &&
	      ISFINITE(ctl_data.body_z_rate) &&
	      ISFINITE(ctl_data.yaw_rate_setpoint) &&
	      ISFINITE(ctl_data.airspeed_min) &&
	      ISFINITE(ctl_data.airspeed_max) &&
	      ISFINITE(ctl_data.scaler))) {
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;

	if (!lock_integrator && _k_i > 0.0f) {

		float id = _rate_error * dt * ctl_data.scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);

		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		/* add and constrain */
		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
	}

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
		       + _integrator;  //scaler is proportional to 1/airspeed

	return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
				 float airspeed_min, float airspeed_max, float aspeed)
{
	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();

	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;

	return 0.0f;
}
Ejemplo n.º 5
0
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(ISFINITE(ctl_data.body_z_rate) &&
	      ISFINITE(ctl_data.groundspeed) &&
	      ISFINITE(ctl_data.groundspeed_scaler))) {
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	/* input conditioning */
	float min_speed = 1.0f;

	/* Calculate body angular rate error */
	_rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error

	if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {

		float id = _rate_error * dt * ctl_data.groundspeed_scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);

		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		/* add and constrain */
		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
	}

	/* Apply PI rate controller and store non-limited output */
	_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
		       ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (_rate_error * _k_p + _integrator);

	return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	switch (_coordinated_method) {
	case COORD_METHOD_OPEN:
	case COORD_METHOD_CLOSEACC:
		return control_bodyrate_impl(ctl_data);

	default:
		static hrt_abstime last_print = 0;

		if (ecl_elapsed_time(&last_print) > 5e6) {
			warnx("invalid param setting FW_YCO_METHOD");
			last_print = ecl_absolute_time();
		}
	}

	return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
	switch (_coordinated_method) {
	case COORD_METHOD_OPEN:
		return control_attitude_impl_openloop(ctl_data);

	case COORD_METHOD_CLOSEACC:
		return control_attitude_impl_accclosedloop(ctl_data);

	default:
		static hrt_abstime last_print = 0;

		if (ecl_elapsed_time(&last_print) > 5e6) {
			warnx("invalid param setting FW_YCO_METHOD");
			last_print = ecl_absolute_time();
		}
	}

	return _rate_setpoint;
}
Ejemplo n.º 8
0
float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(ctl_data.pitch) &&
	      isfinite(ctl_data.roll_rate) &&
	      isfinite(ctl_data.yaw_rate) &&
	      isfinite(ctl_data.yaw_rate_setpoint) &&
	      isfinite(ctl_data.airspeed_min) &&
	      isfinite(ctl_data.airspeed_max) &&
	      isfinite(ctl_data.scaler))) {
		perf_count(_nonfinite_input_perf);
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	/* input conditioning */
	float airspeed = ctl_data.airspeed;

	if (!isfinite(airspeed)) {
		/* airspeed is NaN, +- INF or not available, pick center of band */
		airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);

	} else if (airspeed < ctl_data.airspeed_min) {
		airspeed = ctl_data.airspeed_min;
	}

	/* Transform setpoint to body angular rates (jacobian) */
	_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;

	/* Transform estimation to body angular rates (jacobian) */
	float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate;

	/* Calculate body angular rate error */
	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error

	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {

		float id = _rate_error * dt * ctl_data.scaler;

		/*
		* anti-windup: do not allow integrator to increase if actuator is at limit
		*/
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);

		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
		       + integrator_constrained;  //scaler is proportional to 1/airspeed

	return math::constrain(_last_output, -1.0f, 1.0f);
}
Ejemplo n.º 9
0
float ECL_PitchController::control_bodyrate(const ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(PX4_ISFINITE(ctl_data.roll) &&
	      PX4_ISFINITE(ctl_data.pitch) &&
	      PX4_ISFINITE(ctl_data.pitch_rate) &&
	      PX4_ISFINITE(ctl_data.yaw_rate) &&
	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
	      PX4_ISFINITE(ctl_data.airspeed_min) &&
	      PX4_ISFINITE(ctl_data.airspeed_max) &&
	      PX4_ISFINITE(ctl_data.scaler))) {
		perf_count(_nonfinite_input_perf);
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	/* Transform setpoint to body angular rates (jacobian) */
	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;

	/* apply turning offset to desired bodyrate setpoint*/
	/* flying inverted (wings upside down)*/
	bool inverted = false;
	float constrained_roll;
	/* roll is used as feedforward term and inverted flight needs to be considered */
	if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
		/* not inverted, but numerically still potentially close to infinity */
		constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));

	} else {
		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
		inverted = true;
		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
		if (ctl_data.roll > 0.0f) {
			/* right hemisphere */
			constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));

		} else {
			/* left hemisphere */
			constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
		}
	}

	/* input conditioning */
	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);

	/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
	   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
	   Availible on google books 8/11/2015: 
	   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
	float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
				  		tanf(constrained_roll) * sinf(constrained_roll)));

	if (inverted) {
		body_fixed_turn_offset = -body_fixed_turn_offset;
	}

	/* Finally add the turn offset to your bodyrate setpoint*/
	_bodyrate_setpoint += body_fixed_turn_offset;


	_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;

	if (!lock_integrator && _k_i > 0.0f) {

		float id = _rate_error * dt * ctl_data.scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);

		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

    _dif_rate_error = _rate_error - _last_rate_error;
    _last_rate_error = _rate_error;

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
		       + integrator_constrained;  //scaler is proportional to 1/airspeed
//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
//	warnx("roll: _last_output %.4f", (double)_last_output);
    fp = open(PX4_ROOTFSDIR"/fs/microsd/log/output_pitch.csv", O_CREAT | O_WRONLY | O_DSYNC | O_APPEND);
    int bytes = sprintf(buffer, "%.6f,%.6f,%.6f\n", (double)_rate_error, (double)_dif_rate_error, (double)math::constrain(_last_output, -1.0f, 1.0f));
    write(fp, buffer, bytes);
    close(fp);
	return math::constrain(_last_output, -1.0f, 1.0f);
}
Ejemplo n.º 10
0
float ECL_RollController::control_bodyrate(float pitch,
		float roll_rate, float yaw_rate,
		float yaw_rate_setpoint,
		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	if (dt_micros > 500000)
		lock_integrator = true;

//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
	float k_ff = 0; //xxx: param

	/* input conditioning */
//	warnx("airspeed pre %.4f", (double)airspeed);
	if (!isfinite(airspeed)) {
		/* airspeed is NaN, +- INF or not available, pick center of band */
		airspeed = 0.5f * (airspeed_min + airspeed_max);
	} else if (airspeed < airspeed_min) {
		airspeed = airspeed_min;
	}


	/* Transform setpoint to body angular rates */
	_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian

	/* Transform estimation to body angular rates */
	float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian

	/* Calculate body angular rate error */
	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error

	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {

		float id = _rate_error * dt;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);
		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);

	/* Apply PI rate controller and store non-limited output */
	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed

	return math::constrain(_last_output, -1.0f, 1.0f);
}