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; }