float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(ISFINITE(ctl_data.yaw_setpoint) &&
ISFINITE(ctl_data.yaw))) {
return _rate_setpoint;
}
/* Calculate the error */
float yaw_error = _wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = yaw_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
}
return _rate_setpoint;
}
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 _wrap_360(float bearing)
{
/* value is inf or NaN */
if (!ISFINITE(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= 360.0f) {
bearing -= 360.0f;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += 360.0f;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
float _wrap_2pi(float bearing)
{
/* value is inf or NaN */
if (!ISFINITE(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
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);
}
cairo_bool_t
_cairo_matrix_is_invertible (const cairo_matrix_t *matrix)
{
double det;
det = _cairo_matrix_compute_determinant (matrix);
return ISFINITE (det) && det != 0.;
}
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(ISFINITE(ctl_data.pitch_setpoint) &&
ISFINITE(ctl_data.roll) &&
ISFINITE(ctl_data.pitch) &&
ISFINITE(ctl_data.airspeed))) {
ECL_WARN("not controlling pitch");
return _rate_setpoint;
}
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
return _rate_setpoint;
}
/**
* _cairo_matrix_compute_basis_scale_factors:
* @matrix: a matrix
* @basis_scale: the scale factor in the direction of basis
* @normal_scale: the scale factor in the direction normal to the basis
* @x_basis: basis to use. X basis if true, Y basis otherwise.
*
* Computes |Mv| and det(M)/|Mv| for v=[1,0] if x_basis is true, and v=[0,1]
* otherwise, and M is @matrix.
*
* Return value: the scale factor of @matrix on the height of the font,
* or 1.0 if @matrix is %NULL.
**/
cairo_status_t
_cairo_matrix_compute_basis_scale_factors (const cairo_matrix_t *matrix,
double *basis_scale, double *normal_scale,
cairo_bool_t x_basis)
{
double det;
det = _cairo_matrix_compute_determinant (matrix);
if (! ISFINITE (det))
return _cairo_error (CAIRO_STATUS_INVALID_MATRIX);
if (det == 0)
{
*basis_scale = *normal_scale = 0;
}
else
{
double x = x_basis != 0;
double y = x == 0;
double major, minor;
cairo_matrix_transform_distance (matrix, &x, &y);
major = hypot (x, y);
/*
* ignore mirroring
*/
if (det < 0)
det = -det;
if (major)
minor = det / major;
else
minor = 0.0;
if (x_basis)
{
*basis_scale = major;
*normal_scale = minor;
}
else
{
*basis_scale = minor;
*normal_scale = major;
}
}
return CAIRO_STATUS_SUCCESS;
}
/**
* cairo_matrix_invert:
* @matrix: a #cairo_matrix_t
*
* Changes @matrix to be the inverse of its original value. Not
* all transformation matrices have inverses; if the matrix
* collapses points together (it is <firstterm>degenerate</firstterm>),
* then it has no inverse and this function will fail.
*
* Returns: If @matrix has an inverse, modifies @matrix to
* be the inverse matrix and returns %CAIRO_STATUS_SUCCESS. Otherwise,
* returns %CAIRO_STATUS_INVALID_MATRIX.
**/
cairo_status_t
cairo_matrix_invert (cairo_matrix_t *matrix)
{
double det;
/* Simple scaling|translation matrices are quite common... */
if (matrix->xy == 0. && matrix->yx == 0.) {
matrix->x0 = -matrix->x0;
matrix->y0 = -matrix->y0;
if (matrix->xx != 1.) {
if (matrix->xx == 0.)
return _cairo_error (CAIRO_STATUS_INVALID_MATRIX);
matrix->xx = 1. / matrix->xx;
matrix->x0 *= matrix->xx;
}
if (matrix->yy != 1.) {
if (matrix->yy == 0.)
return _cairo_error (CAIRO_STATUS_INVALID_MATRIX);
matrix->yy = 1. / matrix->yy;
matrix->y0 *= matrix->yy;
}
return CAIRO_STATUS_SUCCESS;
}
/* inv (A) = 1/det (A) * adj (A) */
det = _cairo_matrix_compute_determinant (matrix);
if (! ISFINITE (det))
return _cairo_error (CAIRO_STATUS_INVALID_MATRIX);
if (det == 0)
return _cairo_error (CAIRO_STATUS_INVALID_MATRIX);
_cairo_matrix_compute_adjoint (matrix);
_cairo_matrix_scalar_multiply (matrix, 1 / det);
return CAIRO_STATUS_SUCCESS;
}
/**
* cairo_matrix_invert:
* @matrix: a #cairo_matrix_t
*
* Changes @matrix to be the inverse of it's original value. Not
* all transformation matrices have inverses; if the matrix
* collapses points together (it is <firstterm>degenerate</firstterm>),
* then it has no inverse and this function will fail.
*
* Returns: If @matrix has an inverse, modifies @matrix to
* be the inverse matrix and returns %CAIRO_STATUS_SUCCESS. Otherwise,
* returns %CAIRO_STATUS_INVALID_MATRIX.
**/
cairo_status_t
cairo_matrix_invert (cairo_matrix_t *matrix)
{
/* inv (A) = 1/det (A) * adj (A) */
double det;
det = _cairo_matrix_compute_determinant (matrix);
if (! ISFINITE (det))
return _cairo_error (CAIRO_STATUS_INVALID_MATRIX);
if (det == 0)
return _cairo_error (CAIRO_STATUS_INVALID_MATRIX);
_cairo_matrix_compute_adjoint (matrix);
_cairo_matrix_scalar_multiply (matrix, 1 / det);
return CAIRO_STATUS_SUCCESS;
}
int myarmsMH(double xl, double xr,
double (*myfunc)(double x, void *mydata),
void *mydata, double *xval, char *label,
int doMH) {
double result = *xval;
double *resvec = NULL;
double startval = *xval;
int errcode;
if ( fabs(*xval-xr)/(xr)<0.00001 ) {
*xval = xr * 0.999 + xl * 0.001;
}
if ( fabs(*xval-xl)/xl<0.00001 ) {
*xval = xl * 0.999 + xr * 0.001;
}
myarms_evals = 0;
if ( doMH ) {
resvec = malloc(sizeof(resvec[0])*NSAMP);
errcode = myarms_simple(6, &xl, &xr, myfunc, mydata, doMH, xval,
resvec, NSAMP);
result = resvec[NSAMP-1];
free(resvec);
} else
errcode = myarms_simple(6, &xl, &xr, myfunc, mydata, 0, xval, &result, 1);
/*
* 1007, 1003 is out of bounds
*/
if ( errcode && errcode!=1007 && errcode!=1003 && errcode!=2001
&& (errcode!=2000 || startval!=result ) ) {
yap_quit(" myarmsMH(%lf,%s)->%d = %lf,%lf%s->%lf, w %d calls, quitting\n",
startval, label, errcode,
myarms_last, result, (!ISFINITE(result))?"(inf)":"",
*xval, myarms_evals);
}
if ( errcode==1007 || errcode==1003 ) {
/*
* hit bounds, for safety use start val
*/
yap_message(" error myarmsMH(%lf,%s)->%d = %lf,%lf%s->%lf, w %d calls, quitting\n",
startval, label, errcode,
myarms_last, result, (!ISFINITE(result))?"(inf)":"",
*xval, myarms_evals);
result = startval;
}
if ( errcode==2001 ) {
/*
* too many calls in Metrop-Hastings
*/
yap_message(" error myarmsMH(%lf,%s)->%d = %lf,%lf%s->%lf, w %d calls, quitting\n",
startval, label, errcode,
myarms_last, result, (!ISFINITE(result))?"(inf)":"",
*xval, myarms_evals);
}
/*
* note, sometimes the value is returned
* unchanged .... seems to be when the
* posterior is really focussed
*/
if ( verbose>1 || !ISFINITE(result) || startval==result )
yap_message(" myarmsMH(%s) = %lf%s<-%lf, w %d calls %s\n",
label,
result, (!ISFINITE(result))?"(inf)":"",
*xval, myarms_evals,
(startval==result)?"UNCHANGED":"");
if ( ISFINITE(result) ) {
if ( result<xl )
result = xl;
else if ( result>xr )
result = xr;
*xval = result;
}
if ( !ISFINITE(result) ) {
return 1;
}
return 0;
}
//Setup camera/modelview to view a model with enclosing cuboid defined by min[X,Y,Z] and max[X,Y,Z]
bool View::init(bool force, float* newmin, float* newmax)
{
if (!newmin) newmin = min;
if (!newmax) newmax = max;
for (int i=0; i<3; i++)
{
//Invalid bounds! Skip
if (!ISFINITE(newmin[i]) || !ISFINITE(newmax[i])) return false;
//If bounds changed, reset focal point to default
//(causes jitter when switching timesteps - do we really want this?)
if (min[i] != newmin[i] || max[i] != newmax[i]) focal_point[i] = default_focus[i];
min[i] = newmin[i];
max[i] = newmax[i];
dims[i] = fabs(max[i] - min[i]);
//Fallback focal point and rotation centre = model bounding box centre point
if (focal_point[i] == FLT_MIN) focal_point[i] = min[i] + dims[i] / 2.0f;
rotate_centre[i] = focal_point[i];
}
//Save magnitude of dimensions
model_size = sqrt(dotProduct(dims,dims));
if (model_size == 0 || !ISFINITE(model_size)) return false;
//Check and calculate near/far clip planes
float near_clip = properties["near"];
float far_clip = properties["far"];
checkClip(near_clip, far_clip);
if (max[2] > min[2]+FLT_EPSILON) is3d = true;
else is3d = false;
debug_print("Model size %f dims: %f,%f,%f - %f,%f,%f (scale %f,%f,%f) 3d? %s\n",
model_size, min[0], min[1], min[2], max[0], max[1], max[2], scale[0], scale[1], scale[2], (is3d ? "yes" : "no"));
//Auto-cam etc should only be processed once... and only when viewport size has been set
if ((force || !initialised) && width > 0 && height > 0)
{
//Only flag correctly initialised after focal point set (have model bounds)
initialised = true;
projection(EYE_CENTRE);
//Default translation by model size
if (model_trans[2] == 0)
model_trans[2] = -model_size;
// Initial zoom to fit
// NOTE without (int) cast properties["zoomstep"] == 0 here always evaluated to false!
if ((int)properties["zoomstep"] == 0) zoomToFit();
debug_print(" Auto cam: (Viewport %d x %d) (Model: %f x %f x %f)\n", width, height, dims[0], dims[1], dims[2]);
debug_print(" Looking At: %f,%f,%f\n", focal_point[0], focal_point[1], focal_point[2]);
debug_print(" Rotate Origin: %f,%f,%f\n", rotate_centre[0], rotate_centre[1], rotate_centre[2]);
debug_print(" Clip planes: near %f far %f. Focal length %f Eye separation ratio: %f\n", (float)properties["near"], (float)properties["far"], focal_length, eye_sep_ratio);
debug_print(" Translate: %f,%f,%f\n", model_trans[0], model_trans[1], model_trans[2]);
//Apply changes to view
apply();
}
return true;
}
