/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if not saturated on low limit and if motor commands are not saturated */
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
_rates_prev = rates;
/* update integral only if not saturated on low limit */
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
}
static void initmodel(double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {
int _i; double _save;{
h_q = h_q0;
m_q = m_q0;
{
ena = 67.0 ;
temperature = celsius + 273.15 ;
rates ( _threadargs_ ) ;
rates ( _threadargs_ ) ;
m_q = m_inf ;
h_q = h_inf ;
}
}
}
/*CVODE*/
static int _ode_spec1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {int _reset = 0; {
rates ( _threadargs_ ) ;
Dm_q = rate_m_q ;
Dh_q = rate_h_q ;
}
return _reset;
}
int main()
{
double rate = 0.0;
double vol = 0.1;
int N = 3;
int M = 1;
int b = 700;
double maturity = 1.0;
double initialSpot = 100.0;
double timeStep = (maturity / (double)N);
double m = timeStep*(rate - vol*vol / 2.0);
//std::ofstream fichier("C:\\Users\\Lucien\\Desktop\\e.txt", std::ios::out | std::ios::trunc);
for (int i = 0; i < 40; i++)
{
double initialSpots[1]{80 + (double)i};
//False ditribution in the estimator
lognormal f(m, vol*sqrt(timeStep));
lognormal g(0.0, 5.0*vol);
normalGen gGenerator(ENG(time(0)), normalDist(0.0, 5.0*vol));
RandomGenerationTool kit(f, gGenerator, g);
BlackScholesPathGenerator paths(rate, vol, N, b, M, maturity, kit);
BlackScholesRate rates(N, maturity, rate);
AmericanPutPayoff payoff(100.0, maturity, N, b, M);
Estimator estimator(paths, payoff, rates);
std::cout << estimator.computePrice(initialSpots) << std::endl;
}
//fichier.close();
system("pause");
}
/*END CVODE*/
static int states (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) { {
rates ( _threadargscomma_ v ) ;
n = n + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / taun)))*(- ( ( ( ninf ) ) / taun ) / ( ( ( ( - 1.0) ) ) / taun ) - n) ;
l = l + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / taul)))*(- ( ( ( linf ) ) / taul ) / ( ( ( ( - 1.0) ) ) / taul ) - l) ;
}
return 0;
}
static int _ode_matsol1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {
rates ( _threadargscomma_ v , cai ) ;
Dn = Dn / (1. - dt*( ( ( ( - 1.0 ) ) ) / tau_n )) ;
Dh = Dh / (1. - dt*( ( ( ( - 1.0 ) ) ) / tau_h )) ;
Dc = Dc / (1. - dt*( ( ( ( - 1.0 ) ) ) / tau_c )) ;
return 0;
}
/*END CVODE*/
static int states (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) { {
rates ( _threadargs_ ) ;
m_q = m_q - dt*(- ( rate_m_q ) ) ;
h_q = h_q - dt*(- ( rate_h_q ) ) ;
}
return 0;
}
/*CVODE*/
static int _ode_spec1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {int _reset = 0; {
rates ( _threadargscomma_ v ) ;
DX = ( Xinf - X ) / Xtau ;
DY = ( Yinf - Y ) / Ytau ;
}
return _reset;
}
static int _ode_matsol1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {
rates ( _threadargscomma_ v ) ;
Dm = Dm / (1. - dt*( ( ( ( - 1.0 ) ) ) / mtau )) ;
Dh = Dh / (1. - dt*( ( ( ( - 1.0 ) ) ) / htau )) ;
Dn = Dn / (1. - dt*( ( ( ( - 1.0 ) ) ) / ntau )) ;
return 0;
}
/*CVODE*/
static int _ode_spec1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {int _reset = 0; {
rates ( _threadargscomma_ v ) ;
Dm = ( minf - m ) / mtau ;
Dh = ( hinf - h ) / htau ;
}
return _reset;
}
/*CVODE*/
static int _ode_spec1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {int _reset = 0; {
rates ( _threadargs_ ) ;
Dm = ( mInf - m ) / mTau ;
Dh = ( hInf - h ) / hTau ;
}
return _reset;
}
/*END CVODE*/
static int states (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) { {
rates ( _threadargs_ ) ;
m = m + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / mTau)))*(- ( ( ( mInf ) ) / mTau ) / ( ( ( ( - 1.0) ) ) / mTau ) - m) ;
h = h + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / hTau)))*(- ( ( ( hInf ) ) / hTau ) / ( ( ( ( - 1.0) ) ) / hTau ) - h) ;
}
return 0;
}
static int _ode_matsol1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {
rates ( _threadargscomma_ v ) ;
DX = DX / (1. - dt*( ( ( ( - 1.0 ) ) ) / Xtau )) ;
DY = DY / (1. - dt*( ( ( ( - 1.0 ) ) ) / Ytau )) ;
DZ = DZ / (1. - dt*( ( ( ( - 1.0 ) ) ) / Ztau )) ;
return 0;
}
/*CVODE*/
static int _ode_spec1 () {_reset=0;
{
rates ( _threadargscomma_ v ) ;
Dn = ( ninf - n ) / taun ;
}
return _reset;
}
/*END CVODE*/
static int states () {_reset=0;
{
rates ( _threadargscomma_ v ) ;
n = n + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / taun)))*(- ( ( ( ninf ) ) / taun ) / ( ( ( ( - 1.0) ) ) / taun ) - n) ;
}
return 0;
}
static int _f_trates ( double _lv ) {
double _ltinc ;
rates ( _threadargscomma_ _lv ) ;
_ltinc = - dt * _zq10 ;
_znexp = 1.0 - exp ( _ltinc / ntau ) ;
_zpexp = 1.0 - exp ( _ltinc / ptau ) ;
return 0; }
/*END CVODE*/
static int states (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) { {
rates ( _threadargscomma_ v ) ;
X = X + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / Xtau)))*(- ( ( ( Xinf ) ) / Xtau ) / ( ( ( ( - 1.0) ) ) / Xtau ) - X) ;
Y = Y + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / Ytau)))*(- ( ( ( Yinf ) ) / Ytau ) / ( ( ( ( - 1.0) ) ) / Ytau ) - Y) ;
}
return 0;
}
/*CVODE*/
static int _ode_spec1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {int _reset = 0; {
rates ( _threadargscomma_ v ) ;
Dn = ( ninf - n ) / taun ;
Dl = ( linf - l ) / taul ;
}
return _reset;
}
/*END CVODE*/
static int states () {_reset=0;
{
rates ( _threadargscomma_ v ) ;
n = n + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / tau_n)))*(- ( ( ( inf_n ) ) / tau_n ) / ( ( ( ( - 1.0) ) ) / tau_n ) - n) ;
h = h + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / tau_h)))*(- ( ( ( inf_h ) ) / tau_h ) / ( ( ( ( - 1.0) ) ) / tau_h ) - h) ;
}
return 0;
}
/*CVODE*/
static int _ode_spec1 () {_reset=0;
{
rates ( _threadargscomma_ v ) ;
Dn = ( inf_n - n ) / tau_n ;
Dh = ( inf_h - h ) / tau_h ;
}
return _reset;
}
/*CVODE*/
static int _ode_spec1 (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {int _reset = 0; {
rates ( _threadargscomma_ v , cai ) ;
Dn = ( inf_n - n ) / tau_n ;
Dh = ( inf_h - h ) / tau_h ;
Dc = ( inf_c - c ) / tau_c ;
}
return _reset;
}
/*END CVODE*/
static int states (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) { {
rates ( _threadargscomma_ v ) ;
m = m + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / mtau)))*(- ( ( ( minf ) ) / mtau ) / ( ( ( ( - 1.0) ) ) / mtau ) - m) ;
h = h + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / htau)))*(- ( ( ( hinf ) ) / htau ) / ( ( ( ( - 1.0) ) ) / htau ) - h) ;
n = n + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / ntau)))*(- ( ( ( ninf ) ) / ntau ) / ( ( ( ( - 1.0) ) ) / ntau ) - n) ;
}
return 0;
}
//-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
// GetPossibleFrameRateByIndex()
//-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
Float64 Stream::GetPossibleFrameRateByIndex(UInt32 index) const
{
UInt32 numberRates = GetNumberAllPossibleFrameRates();
ThrowIf(index >= numberRates, CAException(kCMIOHardwareIllegalOperationError), "CMIO::DALA::Stream::GetFrameRateByIndex: index out of range");
CAAutoArrayDelete<Float64> rates(numberRates);
GetAllPossibleFrameRates(numberRates, rates);
return rates[index];
}
/*END CVODE*/
static int states (double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) { {
rates ( _threadargscomma_ v , cai ) ;
n = n + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / tau_n)))*(- ( ( ( inf_n ) ) / tau_n ) / ( ( ( ( - 1.0) ) ) / tau_n ) - n) ;
h = h + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / tau_h)))*(- ( ( ( inf_h ) ) / tau_h ) / ( ( ( ( - 1.0) ) ) / tau_h ) - h) ;
c = c + (1. - exp(dt*(( ( ( - 1.0 ) ) ) / tau_c)))*(- ( ( ( inf_c ) ) / tau_c ) / ( ( ( ( - 1.0) ) ) / tau_c ) - c) ;
}
return 0;
}
static void initmodel(double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {
int _i; double _save;{
n = n0;
{
rates ( _threadargscomma_ v ) ;
n = ninf ;
}
}
}
static void _hoc_rates(void) {
double _r;
double* _p; Datum* _ppvar; Datum* _thread; _NrnThread* _nt;
if (_extcall_prop) {_p = _extcall_prop->param; _ppvar = _extcall_prop->dparam;}else{ _p = (double*)0; _ppvar = (Datum*)0; }
_thread = _extcall_thread;
_nt = nrn_threads;
_r = 1.;
rates ( _p, _ppvar, _thread, _nt, *getarg(1) );
hoc_retpushx(_r);
}
static void initmodel(double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {
int _i; double _save;{
z = z0;
{
rates ( _threadargscomma_ cai ) ;
z = zInf ;
}
}
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* throttle pid attenuation factor */
float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_params.rate_ff.emult(_rates_sp);
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if not saturated on low limit and if motor commands are not saturated */
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
/* if the axis is the yaw axis, do not update the integral if the limit is hit */
!((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
_rates_int(i) = rate_i;
}
}
}
}
}
static void initmodel(double* _p, Datum* _ppvar, Datum* _thread, _NrnThread* _nt) {
int _i; double _save;{
X = X0;
{
eca = 80.0 ;
rates ( _threadargscomma_ v ) ;
X = Xinf ;
}
}
}
