void ant_servo_set ( uint16_t value1_us, uint16_t value2_us) {
/* code pour regler la valeur en ms a l'etat haut du signal PWM */
Bound(value1_us, MIN_SERVO, MAX_SERVO);
OCR1A = TICKS_OF_US(value1_us);
Bound(value2_us, MIN_SERVO, MAX_SERVO);
OCR1B = TICKS_OF_US(value2_us);
}
/**
* Run the throttle curve and generate the output throttle and pitch
* This depends on the FMODE(flight mode) and TRHUST command
*/
void throttle_curve_run(bool_t motors_on, pprz_t in_cmd[])
{
// Calculate the mode value from the switch
int8_t mode = ((float)(in_cmd[COMMAND_FMODE] + MAX_PPRZ) / THROTTLE_CURVE_SWITCH_VAL);
Bound(mode, 0, THROTTLE_CURVES_NB - 1);
throttle_curve.mode = mode;
// Check if we have multiple points or a single point
struct curve_t curve = throttle_curve.curves[mode];
if (curve.nb_points == 1) {
throttle_curve.throttle = curve.throttle[0];
throttle_curve.collective = curve.collective[0];
} else {
// Calculate the left point on the curve we need to use
uint16_t curve_range = (MAX_PPRZ / (curve.nb_points - 1));
int8_t curve_p = ((float)in_cmd[COMMAND_THRUST] / curve_range);
Bound(curve_p, 0, curve.nb_points - 1);
// Calculate the throttle and pitch value
uint16_t x = in_cmd[COMMAND_THRUST] - curve_p * curve_range;
throttle_curve.throttle = curve.throttle[curve_p]
+ ((curve.throttle[curve_p + 1] - curve.throttle[curve_p]) * x / curve_range);
throttle_curve.collective = curve.collective[curve_p]
+ ((curve.collective[curve_p + 1] - curve.collective[curve_p]) * x / curve_range);
}
// Only set throttle if motors are on
if (!motors_on) {
throttle_curve.throttle = 0;
}
}
void stabilization_attitude_run(bool_t enable_integrator)
{
/*
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct Int32Quat att_err;
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
int32_quat_wrap_shortest(&att_err);
int32_quat_normalize(&att_err);
/* rate error */
const struct Int32Rates rate_ref_scaled = {
OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))
};
struct Int32Rates rate_err;
struct Int32Rates *body_rate = stateGetBodyRates_i();
RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
#define INTEGRATOR_BOUND 100000
/* integrated error */
if (enable_integrator) {
stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE;
stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE;
stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE;
Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
} else {
/* reset accumulator */
int32_quat_identity(&stabilization_att_sum_err_quat);
}
/* compute the feed forward command */
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel);
/* compute the feed back command */
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);
/* sum feedforward and feedback */
stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
void OrenNayarBlinnShader::Update(TimeValue t, Interval &valid) {
Point3 p, p2;
if( inUpdate )
return;
inUpdate = TRUE;
if (!ivalid.InInterval(t)) {
ivalid.SetInfinite();
// pblock->GetValue( onb_ambient, t, p, ivalid );
// ambient = LimitColor(Color(p.x,p.y,p.z));
pblock->GetValue( onb_diffuse, t, p, ivalid );
diffuse= LimitColor(Color(p.x,p.y,p.z));
pblock->GetValue( onb_ambient, t, p2, ivalid );
if( lockAD && (p!=p2)){
pblock->SetValue( onb_ambient, t, diffuse);
ambient = diffuse;
} else {
pblock->GetValue( onb_ambient, t, p, ivalid );
ambient = Bound(Color(p.x,p.y,p.z));
}
pblock->GetValue( onb_specular, t, p2, ivalid );
if( lockDS && (p!=p2)){
pblock->SetValue( onb_specular, t, diffuse);
specular = diffuse;
} else {
pblock->GetValue( onb_specular, t, p, ivalid );
specular = Bound(Color(p.x,p.y,p.z));
}
// pblock->GetValue( onb_specular, t, p, ivalid );
// specular = LimitColor(Color(p.x,p.y,p.z));
pblock->GetValue( onb_glossiness, t, glossiness, ivalid );
LIMIT0_1(glossiness);
pblock->GetValue( onb_specular_level, t, specularLevel, ivalid );
LIMITMINMAX(specularLevel,0.0f,9.99f);
pblock->GetValue( onb_soften, t, softThresh, ivalid);
LIMIT0_1(softThresh);
pblock->GetValue( onb_self_illum_amnt, t, selfIllum, ivalid );
LIMIT0_1(selfIllum);
pblock->GetValue( onb_self_illum_color, t, p, ivalid );
selfIllumClr = LimitColor(Color(p.x,p.y,p.z));
pblock->GetValue( onb_diffuse_level, t, diffLevel, ivalid );
LIMITMINMAX(diffLevel,0.0f, 4.00f);
pblock->GetValue( onb_roughness, t, diffRough, ivalid );
LIMIT0_1(diffRough);
// also get the non-animatables in case changed from scripter or other pblock accessors
pblock->GetValue(onb_ds_lock, t, lockDS, ivalid);
pblock->GetValue(onb_ad_lock, t, lockAD, ivalid);
pblock->GetValue(onb_ad_texlock, t, lockADTex, ivalid);
pblock->GetValue(onb_use_self_illum_color, t, selfIllumClrOn, ivalid);
curTime = t;
}
valid &= ivalid;
inUpdate = FALSE;
}
static void attitude_run_indi(int32_t indi_commands[], struct Int32Quat *att_err, bool_t in_flight)
{
//Calculate required angular acceleration
struct FloatRates *body_rate = stateGetBodyRates_f();
indi.angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- reference_acceleration.rate_p * body_rate->p;
indi.angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- reference_acceleration.rate_q * body_rate->q;
indi.angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
- reference_acceleration.rate_r * body_rate->r;
//Incremented in angular acceleration requires increment in control input
//G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
//It takes care of the angular acceleration caused by the change in rotation rate of the propellers
//(they have significant inertia, see the paper mentioned in the header for more explanation)
indi.du.p = 1.0 / g1.p * (indi.angular_accel_ref.p - indi.filtered_rate_deriv.p);
indi.du.q = 1.0 / g1.q * (indi.angular_accel_ref.q - indi.filtered_rate_deriv.q);
indi.du.r = 1.0 / (g1.r + g2) * (indi.angular_accel_ref.r - indi.filtered_rate_deriv.r + g2 * indi.du.r);
//add the increment to the total control input
indi.u_in.p = indi.u.p + indi.du.p;
indi.u_in.q = indi.u.q + indi.du.q;
indi.u_in.r = indi.u.r + indi.du.r;
//bound the total control input
Bound(indi.u_in.p, -4500, 4500);
Bound(indi.u_in.q, -4500, 4500);
Bound(indi.u_in.r, -4500, 4500);
//Propagate input filters
//first order actuator dynamics
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
//sensor filter
stabilization_indi_second_order_filter(&indi.u_act_dyn, &indi.udotdot, &indi.udot, &indi.u,
STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
//Don't increment if thrust is off
if (stabilization_cmd[COMMAND_THRUST] < 300) {
FLOAT_RATES_ZERO(indi.u);
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
FLOAT_RATES_ZERO(indi.udot);
FLOAT_RATES_ZERO(indi.udotdot);
} else {
#if STABILIZATION_INDI_USE_ADAPTIVE
#warning "Use caution with adaptive indi. See the wiki for more info"
lms_estimation();
#endif
}
/* INDI feedback */
indi_commands[COMMAND_ROLL] = indi.u_in.p;
indi_commands[COMMAND_PITCH] = indi.u_in.q;
indi_commands[COMMAND_YAW] = indi.u_in.r;
}
Color OrenNayarIllum( Point3& N, Point3& L, Point3& V, float rough, Color& rho, float* pDiffIntensOut, float NL )
{
// float a = NL >= -1.0f ? float( acos( NL / Len(L) )) : AngleBetween( N, L );
// use the non-uniform scale corrected NL
if ( NL < -1.0f )
NL = Dot( N, L );
// float a = float( acos( NL / Len(L) )) ;
float a = 0.0f;
if( NL < 0.9999f )
a = acosf( NL );
a = Bound( a, -Pi*0.49f, Pi*0.49f );
float NV = Dot( N, V );
// need this to accomodate double sided materials
if( NV < 0.0f ){
NV = -NV;
V = -V;
}
float b = 0.0f;
if( NV < 0.9999f )
b = acosf( NV );
MinMax( b, a ); // b gets min, a gets max
//N.V is the length of the projection of v onto n; times N is a vector along N of that length
// V - that pt gives a tangent vector in the plane of N, for measuring phi
Point3 tanV = V - N * NV;
Point3 tanL = L - N * NL;
float w = Len( tanV ) * Len( tanL );
float cosDPhi = (Abs(w) < 1e-4) ? 1.0f : Dot( tanV, tanL ) / w;
cosDPhi = Bound( cosDPhi, -1.0f, 1.0f );
// float bCube = (cosDPhi >= 0.0f) ? 0.0f : Cube( 2.0f * b * PiIvr );
float bCube = (cosDPhi >= 0.0f) ? Cube( 2.0f * -b * PiIvr ) : Cube( 2.0f * b * PiIvr );
// these three can be pre-calc'd for speed
float sigma2 = Sqr( rough );
float sigma3 = sigma2 / (sigma2 + 0.09f);
float c1 = 1.0f - 0.5f * (sigma2 / (sigma2 + 0.33f));
float c2 = 0.45f * sigma3 * (float(sin(a)) - bCube);
float c3 = 0.125f * sigma3 * Sqr( 4.0f * a * b * Pi2Ivr );
float tanB = float( tan(b) );
float tanAB = float( tan( (a+b) * 0.5f ));
tanB = Bound( tanB, -100.0f, 100.0f );
tanAB = Bound( tanAB, -100.0f, 100.0f );
Color o;
float l1 = ( c1 + c2 * cosDPhi * tanB + c3 * (1.0f - Abs( cosDPhi )) * tanAB );
float l2 = 0.17f * (sigma2 / (sigma2 + 0.13f)) * ( 1.0f - cosDPhi * Sqr( 2.0f * b * PiIvr ));
if( pDiffIntensOut )
*pDiffIntensOut = l1+l2;
o.r = l1 * rho.r + l2 * Sqr( rho.r );
o.g = l1 * rho.g + l2 * Sqr( rho.g );
o.b = l1 * rho.b + l2 * Sqr( rho.b );
return UBound( o );
}
void FarPlugin::KeyConfig()
{
//int res = Info.MacroControl(&MainGuid, MCTL_SAVEALL, 0, nullptr);
//
//BindAll();
//ShowMessage(L"", L"Hotkeys binded", FMSG_MB_OK);
//return;
FarDialog & dlg = plugin->Dialogs()[L"KeysDialog"];
dlg.ResetControls();
bool bind = Bound(L"F5") && Bound(L"F6") && Bound(L"ShiftF5") && Bound(L"ShiftF6");
bool altShift = bind && Bound(L"AltShiftF5") && Bound(L"AltShiftF6");
bool ctrlShift = bind && Bound(L"CtrlShiftF5") && Bound(L"CtrlShiftF6");
bool ctrlAlt = bind && Bound(L"CtrlAltF5") && Bound(L"CtrlAltF6");
if (!altShift && !ctrlShift && !ctrlAlt)
altShift = true;
dlg[L"BindToF5"](L"Selected") = bind;
dlg[L"AltShiftF5"](L"Selected") = altShift;
dlg[L"CtrlShiftF5"](L"Selected") = ctrlShift;
dlg[L"CtrlAltF5"](L"Selected") = ctrlAlt;
if (dlg.Execute() == -1)
return;
if (dlg[L"BindToF5"](L"Selected") == bind &&
dlg[L"AltShiftF5"](L"Selected") == altShift &&
dlg[L"CtrlShiftF5"](L"Selected") == ctrlShift &&
dlg[L"CtrlAltF5"](L"Selected") == ctrlAlt)
return;
Unbind(L"F5");
Unbind(L"ShiftF5");
Unbind(L"F6");
Unbind(L"ShiftF6");
Unbind(L"AltShiftF5");
Unbind(L"AltShiftF6");
Unbind(L"CtrlShiftF5");
Unbind(L"CtrlShiftF6");
Unbind(L"CtrlAltF5");
Unbind(L"CtrlAltF6");
if (dlg[L"BindToF5"](L"Selected"))
{
BindAll();
String key;
if (dlg[L"AltShiftF5"](L"Selected"))
key = L"AltShift";
else if (dlg[L"CtrlShiftF5"](L"Selected"))
key = L"CtrlShift";
else if (dlg[L"CtrlAltF5"](L"Selected"))
key = L"CtrlAlt";
Bind(key + L"F5", L"Keys(\"F5\")", L"FileCopyEx3 - Standard copy dialog", 0);
Bind(key + L"F6", L"Keys(\"F6\")", L"FileCopyEx3 - Standard move dialog", 0);
}
}
inline static void h_ctl_cl_loop(void)
{
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
#if (!defined SITL || defined USE_NPS)
struct Int32Vect3 accel_meas_body, accel_ned;
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
VECT3_COPY(accel_ned, (*accel_tmp));
accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
float nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f;
// max load factor to be taken into acount
// to prevent negative flap movement du to negative acceleration
Bound(nz, 1.f, 2.f);
#else
float nz = 1.f;
#endif
#endif
// Compute a corrected airspeed corresponding to the current load factor nz
// with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V,
// the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2,
// thus Vn = V / sqrt(nz)
#if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT
float corrected_airspeed = v_ctl_auto_airspeed_setpoint;
#else
float corrected_airspeed = stateGetAirspeed_f();
#endif
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
corrected_airspeed /= sqrtf(nz);
#endif
Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED);
float cmd = 0.f;
// deadband around NOMINAL_AIRSPEED, rest linear
if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) {
cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED);
}
else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) {
cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED);
}
// no control in manual mode
if (pprz_mode == PPRZ_MODE_MANUAL) {
cmd = 0.f;
}
// bound max flap angle
Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL);
// from percent to pprz
cmd = cmd * MAX_PPRZ;
h_ctl_flaps_setpoint = TRIM_PPRZ(cmd);
}
void rotorcraft_cam_periodic(void)
{
switch (rotorcraft_cam_mode) {
case ROTORCRAFT_CAM_MODE_NONE:
#if ROTORCRAFT_CAM_USE_TILT
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
#endif
#if ROTORCRAFT_CAM_USE_PAN
rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
#endif
break;
case ROTORCRAFT_CAM_MODE_MANUAL:
// nothing to do here, just apply tilt pwm at the end
break;
case ROTORCRAFT_CAM_MODE_HEADING:
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
#if ROTORCRAFT_CAM_USE_PAN
INT32_COURSE_NORMALIZE(rotorcraft_cam_pan);
nav_heading = rotorcraft_cam_pan;
#endif
break;
case ROTORCRAFT_CAM_MODE_WP:
#ifdef ROTORCRAFT_CAM_TRACK_WP
{
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
rotorcraft_cam_pan = int32_atan2(diff.x, diff.y);
nav_heading = rotorcraft_cam_pan;
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
int32_t dist, height;
dist = INT32_VECT2_NORM(diff);
height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
rotorcraft_cam_tilt = int32_atan2(height, dist);
Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
}
#endif
break;
default:
break;
}
#if ROTORCRAFT_CAM_USE_TILT
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
#endif
}
bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
if (init){
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed; // set target speed for approach
#endif
init = FALSE;
}
float final_x = WaypointX(_td) - WaypointX(_tod);
float final_y = WaypointY(_td) - WaypointY(_tod);
float final2 = Max(final_x * final_x + final_y * final_y, 1.);
float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress,-1,1);
float nav_final_length = sqrt(final2);
float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
Bound(pre_climb, -5, 0.);
float start_alt = WaypointAlt(_tod);
float diff_alt = WaypointAlt(_td) - start_alt;
float alt = start_alt + nav_final_progress * diff_alt;
Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(v_ctl_altitude_pgain))) // to prevent climbing before intercept
if(nav_final_progress < -0.5) { // for smooth intercept
NavVerticalAltitudeMode(WaypointAlt(_tod), 0); // vertical mode (fly straigt and intercept glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
else {
NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
return TRUE;
} // end of gls()
void estimator_update_state_infrared( void ) {
float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON ?
estimator_rad_of_ir :
ir_rad_of_ir);
#ifdef IR_360
if (ir_360) { /* 360° estimation */
/* 250 us for the whole block */
float tmp_ir_roll = ir_roll * IR_360_LATERAL_CORRECTION;
float tmp_ir_pitch = ir_pitch * IR_360_LONGITUDINAL_CORRECTION;
float tmp_ir_top = ir_top * IR_360_VERTICAL_CORRECTION;
estimator_phi = atan2(tmp_ir_roll, tmp_ir_top) - ir_roll_neutral;
estimator_phi = correct_angle(estimator_phi, ir_estimated_phi_pi_4, ir_estimated_phi_minus_pi_4);
estimator_theta = atan2(tmp_ir_pitch, tmp_ir_top) - ir_pitch_neutral;
estimator_theta = correct_angle(estimator_theta, ir_estimated_theta_pi_4, ir_estimated_theta_minus_pi_4);
if (estimator_theta < -M_PI_2)
estimator_theta += M_PI;
else if (estimator_theta > M_PI_2)
estimator_theta -= M_PI;
} else
#endif /* IR_360 */
{
ir_top = Max(ir_top, 1);
float c = rad_of_ir*(1-z_contrast_mode)+z_contrast_mode*((float)IR_RAD_OF_IR_CONTRAST/fabs(ir_top));
estimator_phi = c * ir_roll - ir_roll_neutral;
estimator_theta = c * ir_pitch - ir_pitch_neutral;
/* infrared compensation */
#if defined IR_CORRECTION_LEFT && defined IR_CORRECTION_RIGHT
if (estimator_phi >= 0)
estimator_phi *= ir_correction_right;
else
estimator_phi *= ir_correction_left;
#endif
#if defined IR_CORRECTION_UP && defined IR_CORRECTION_DOWN
if (estimator_theta >= 0)
estimator_theta *= ir_correction_up;
else
estimator_theta *= ir_correction_down;
#endif
Bound(estimator_phi, -M_PI_2, M_PI_2);
Bound(estimator_theta, -M_PI_2, M_PI_2);
}
}
/** normalize superbitrf rc_values to radio values */
static void superbitrf_rc_normalize(int16_t *in, int16_t *out, uint8_t count)
{
uint8_t i;
for (i = 0; i < count; i++) {
if (i == RADIO_THROTTLE) {
out[i] = (in[i] + MAX_PPRZ) / 2;
Bound(out[i], 0, MAX_PPRZ);
} else {
out[i] = in[i];
Bound(out[i], MIN_PPRZ, MAX_PPRZ);
}
}
}
Tp Knap<Tw, Tp>::MaxProfitKnapsack()
{// Return profit of best knapsack filling.
// Set bestx[i] = 1 iff object i is in knapsack in
// best filling. Use max-profit branch and bound.
// define a max heap for up to
// 1000 live nodes
H = new MaxHeap<HeapNode<Tp, Tw> > (1000);
// allocate space for bestx
bestx = new int [n+1];
// initialize for level 1 start
int i = 1;
E = 0;
cw = cp = 0;
Tp bestp = 0; // best profit so far
Tp up = Bound(1); // maximum possible profit
// in subtree with root E
// search subset space tree
while (i != n+1) {// not at leaf
// check left child
Tw wt = cw + w[i];
if (wt <= c) {// feasible left child
if (cp+p[i] > bestp) bestp = cp+p[i];
AddLiveNode(up, cp+p[i], cw+w[i], true, i+1);}
up = Bound(i+1);
// check right child
if (up >= bestp) // right child has prospects
AddLiveNode(up, cp, cw, false, i+1);
// get next E-node
HeapNode<Tp, Tw> N;
H->DeleteMax(N); // cannot be empty
E = N.ptr;
cw = N.weight;
cp = N.profit;
up = N.uprofit;
i = N.level;
}
// construct bestx[] by following path
// from E-node E to the root
for (int j = n; j > 0; j--) {
bestx[j] = E->LChild;
E = E->parent;
}
return cp;
}
void StraussShader::Update(TimeValue t, Interval &valid) {
Point3 p;
if (!ivalid.InInterval(t)) {
ivalid.SetInfinite();
pblock->GetValue( st_diffuse, t, p, ivalid );
diffuse= Bound(Color(p.x,p.y,p.z));
pblock->GetValue( st_glossiness, t, glossiness, ivalid );
glossiness = Bound(glossiness );
pblock->GetValue( st_metalness, t, metalness, ivalid );
metalness = Bound(metalness );
}
valid &= ivalid;
}
void attitude_loop( void ) {
#if USE_INFRARED
ahrs_update_infrared();
#endif /* USE_INFRARED */
if (pprz_mode >= PPRZ_MODE_AUTO2)
{
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_pitch_setpoint = nav_pitch;
}
else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
{
v_ctl_climb_loop();
}
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ);
#endif
#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
if (vsupply > 0.) {
v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
}
#endif
h_ctl_pitch_setpoint = v_ctl_pitch_setpoint; // Copy the pitch setpoint from the guidance to the stabilization control
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
if (kill_throttle || (!estimator_flight_time && !launch))
v_ctl_throttle_setpoint = 0;
}
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
/**Directly set the flag indicating to FBW that shared buffer is available*/
inter_mcu_received_ap = TRUE;
#endif
}
inline static void v_ctl_climb_auto_throttle_loop(void)
{
float f_throttle = 0;
float controlled_throttle;
float v_ctl_pitch_of_vz;
// Limit rate of change of climb setpoint (to ensure that airspeed loop can catch-up)
static float v_ctl_climb_setpoint_last = 0;
float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT);
v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
// Pitch control (input: rate of climb error, output: pitch setpoint)
float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
v_ctl_auto_pitch_sum_err += err;
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain *
(err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f());
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) *
v_ctl_auto_groundspeed_pgain;
// Do not allow controlled airspeed below the setpoint
if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) {
v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint;
v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain *
v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop
}
// Airspeed control loop (input: airspeed controlled, output: throttle controlled)
float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f());
v_ctl_auto_airspeed_sum_err += err_airspeed;
BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) *
v_ctl_auto_airspeed_pgain;
// Done, set outputs
Bound(controlled_throttle, 0, v_ctl_auto_throttle_max_cruise_throttle);
f_throttle = controlled_throttle;
v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim;
v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
}
void pid_slew_gaz( void ) {
static pprz_t last_gaz=TRIM_PPRZ(CLIMB_LEVEL_GAZ*MAX_PPRZ);
pprz_t diff_gaz = desired_gaz - last_gaz;
Bound(diff_gaz, -TRIM_PPRZ(CLIMB_MAX_DIFF_GAZ*MAX_PPRZ), TRIM_PPRZ(CLIMB_MAX_DIFF_GAZ*MAX_PPRZ));
desired_gaz = last_gaz + diff_gaz;
last_gaz = desired_gaz;
}
Reduce::Bound Reduce::Bind(Renderer::GenericBuffer& input, Renderer::GenericBuffer& output)
{
std::vector<Renderer::GenericBuffer*> buffers;
buffers.push_back(&input);
for (auto& buffer : mBuffers)
{
buffers.push_back(&buffer);
}
buffers.push_back(&output);
std::vector<Renderer::CommandBuffer::CommandFn> bufferBarriers;
std::vector<Renderer::Work::Bound> bounds;
auto computeSize = MakeComputeSize(mSize);
for (std::size_t i = 0; i < buffers.size() - 1; i++)
{
bounds.emplace_back(mReduce.Bind(computeSize, {*buffers[i], *buffers[i + 1]}));
computeSize = MakeComputeSize(computeSize.WorkSize.x);
vk::Buffer buffer = buffers[i + 1]->Handle();
bufferBarriers.emplace_back([=](vk::CommandBuffer commandBuffer) {
Renderer::BufferBarrier(
buffer, commandBuffer, vk::AccessFlagBits::eShaderWrite, vk::AccessFlagBits::eShaderRead);
});
}
return Bound(mSize, bufferBarriers, std::move(bounds));
}
/**
* outer loop
* \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
*/
void v_ctl_altitude_loop(void)
{
float altitude_pgain_boost = 1.0;
#if USE_AIRSPEED && defined(AGR_CLIMB)
// Aggressive climb mode (boost gain of altitude loop)
if (v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
float dist = fabs(v_ctl_altitude_error);
altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN - 1.0) * (dist - AGR_BLEND_END) /
(AGR_BLEND_START - AGR_BLEND_END);
Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
}
#endif
v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error
+ v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction;
BoundAbs(v_ctl_climb_setpoint, v_ctl_altitude_max_climb);
#ifdef AGR_CLIMB
if (v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
float dist = fabs(v_ctl_altitude_error);
if (dist < AGR_BLEND_END) {
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
} else if (dist > AGR_BLEND_START) {
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_AGRESSIVE;
} else {
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_BLENDED;
}
}
#endif
}
void Triangle::gen_bound_box(){
//set bound box;
Vector3 upper = mat.transform_point(vertices[0].position);
Vector3 lower = upper;
for (size_t i = 1; i < 3; ++i){
Vector3 pos = mat.transform_point(vertices[i].position);
if (pos.x < lower.x) lower.x = pos.x;
if (pos.y < lower.y) lower.y = pos.y;
if (pos.z < lower.z) lower.z = pos.z;
if (pos.x > upper.x) upper.x = pos.x;
if (pos.y > upper.y) upper.y = pos.y;
if (pos.z > upper.z) upper.z = pos.z;
}
// incase the lower and upper is same, because bound box cound not be a plane or a line,
// otherwise there will be some bug in intersect test of bound box
if (lower.x == upper.x) upper.x += EPS;
if (lower.y == upper.y) upper.y += EPS;
if (lower.z == upper.z) upper.z += EPS;
box = Bound(lower, upper);
}
static void bound_commands(void)
{
uint8_t j;
for (j = 0; j < MOTOR_MIXING_NB_MOTOR; j++) {
Bound(motor_mixing.commands[j], MOTOR_MIXING_MIN_MOTOR, MOTOR_MIXING_MAX_MOTOR);
}
}
void StraussShader::AffectReflection(ShadeContext &sc, IllumParams &ip, Color &rClr )
{
float opac = ip.channels[ S_TR ].r;
float g = ip.channels[ S_GL ].r;
float m = ip.channels[ S_MT ].r;
Color Cd = ip.channels[ S_DI ];
float rn = opac - (1.0f - g * g * g) * opac;
// the reflection of the reflection vector is just the view vector
// so dot(v, r) is 1, to any power is still 1
float a, b;
// NB: this has been transformed for existing in-pointing v
float NV = Dot( sc.V(), sc.Normal() );
Point3 R = sc.V() - 2.0f * NV * sc.Normal();
float NR = Dot( sc.Normal(), R );
a = (float)acos( NR ) * OneOverHalfPi;
b = (float)acos( NV ) * OneOverHalfPi;
float fa = F( a );
float j = fa * G( a ) * G( b );
float rj = Bound( rn + (rn+kj)*j );
Color white( 1.0f, 1.0f, 1.0f );
Color Cs = white + m * (1.0f - fa) * (Cd - white);
rClr *= Cs * rj * REFL_BRIGHTNESS_ADJUST;
}
/**
* outer loop
* \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
*/
void v_ctl_altitude_loop( void ) {
float altitude_pgain_boost = 1.0;
#if defined(USE_AIRSPEED) && defined(AGR_CLIMB)
// Aggressive climb mode (boost gain of altitude loop)
if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
float dist = fabs(v_ctl_altitude_error);
altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN-1.0)*(dist-AGR_BLEND_END)/(AGR_BLEND_START-AGR_BLEND_END);
Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
}
#endif
v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint;
v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error
+ v_ctl_altitude_pre_climb;
BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
#ifdef AGR_CLIMB
if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
float dist = fabs(v_ctl_altitude_error);
if (dist < AGR_BLEND_END) {
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
}
else if (dist > AGR_BLEND_START) {
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_AGRESSIVE;
}
else {
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_BLENDED;
}
}
#endif
}
void v_ctl_climb_loop ( void ) {
switch (v_ctl_speed_mode) {
case V_CTL_SPEED_THROTTLE:
// Set pitch
v_ctl_set_pitch();
// Set throttle
v_ctl_set_throttle();
break;
#if USE_AIRSPEED
case V_CTL_SPEED_AIRSPEED:
v_ctl_set_airspeed();
break;
case V_CTL_SPEED_GROUNDSPEED:
v_ctl_set_groundspeed();
v_ctl_set_airspeed();
break;
#endif
default:
controlled_throttle = v_ctl_auto_throttle_cruise_throttle; // ???
break;
}
// Set Pitch output
Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
// Set Throttle output
v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
}
int EQS_UVS2D_TMAI::Coefs( ELEM* elem,
PROJECT* project,
double** estifm,
double* force )
{
if( isFS(elem->flag, ELEM::kDry) ) return 0;
if( this->timegrad )
{
if( isFS(elem->flag, ELEM::kBound) )
{
Bound_dt( elem, project, estifm, force );
}
else
{
Region_dt( elem, project, estifm, force );
}
}
else
{
if( isFS(elem->flag, ELEM::kBound) )
{
Bound( elem, project, estifm, force );
}
else
{
Region( elem, project, estifm, force );
}
}
return 1;
}
inline static void loiter(void) {
float pitch_trim;
static float last_pitch_trim;
#if USE_AIRSPEED
if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) {
pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
} else {
pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1);
}
#else
float throttle_diff = v_ctl_auto_throttle_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle;
if (throttle_diff > 0) {
float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
} else {
float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
}
#endif
// Pitch trim rate limiter
float max_change = (v_ctl_pitch_loiter_trim - v_ctl_pitch_dash_trim) * H_CTL_REF_DT/ PITCH_TRIM_RATE_LIMITER;
Bound(pitch_trim, last_pitch_trim - max_change, last_pitch_trim + max_change);
last_pitch_trim = pitch_trim;
h_ctl_pitch_loop_setpoint += pitch_trim;
}
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
{
struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
VECT2_DIFF(wp_diff, *wp_end, *wp_start);
VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
// go back to metric precision or values are too large
VECT2_COPY(wp_diff_prec, wp_diff);
INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_POS_FRAC);
uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
nav_leg_length = int32_sqrt(leg_length2);
nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
nav_leg_progress += progress;
int32_t prog_2 = nav_leg_length;
Bound(nav_leg_progress, 0, prog_2);
struct Int32Vect2 progress_pos;
VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
VECT2_SUM(navigation_target, *wp_start, progress_pos);
nav_segment_start = *wp_start;
nav_segment_end = *wp_end;
horizontal_mode = HORIZONTAL_MODE_ROUTE;
dist2_to_wp = get_dist2_to_point(wp_end);
}
/**
* This outputs a subpixel window image in grayscale
* Currently only works with Grayscale images as input but could be upgraded to
* also support YUV422 images.
* @param[in] *input Input image (grayscale only)
* @param[out] *output Window output (width and height is used to calculate the window size)
* @param[in] *center Center point in subpixel coordinates
* @param[in] subpixel_factor The subpixel factor per pixel
*/
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint16_t subpixel_factor)
{
uint8_t *input_buf = (uint8_t *)input->buf;
uint8_t *output_buf = (uint8_t *)output->buf;
// Calculate the window size
uint16_t half_window = output->w / 2;
uint16_t subpixel_w = input->w * subpixel_factor;
uint16_t subpixel_h = input->h * subpixel_factor;
// Go through the whole window size in normal coordinates
for (uint16_t i = 0; i < output->w; i++) {
for (uint16_t j = 0; j < output->h; j++) {
// Calculate the subpixel coordinate
uint16_t x = center->x + (i - half_window) * subpixel_factor;
uint16_t y = center->y + (j - half_window) * subpixel_factor;
Bound(x, 0, subpixel_w);
Bound(y, 0, subpixel_h);
// Calculate the original pixel coordinate
uint16_t orig_x = x / subpixel_factor;
uint16_t orig_y = y / subpixel_factor;
// Calculate top left (in subpixel coordinates)
uint16_t tl_x = orig_x * subpixel_factor;
uint16_t tl_y = orig_y * subpixel_factor;
// Check if it is the top left pixel
if (tl_x == x && tl_y == y) {
output_buf[output->w * j + i] = input_buf[input->w * orig_y + orig_x];
} else {
// Calculate the difference from the top left
uint16_t alpha_x = (x - tl_x);
uint16_t alpha_y = (y - tl_y);
// Blend from the 4 surrounding pixels
uint32_t blend = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + orig_x];
blend += alpha_x * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + (orig_x + 1)];
blend += (subpixel_factor - alpha_x) * alpha_y * input_buf[input->w * (orig_y + 1) + orig_x];
blend += alpha_x * alpha_y * input_buf[input->w * (orig_y + 1) + (orig_x + 1)];
// Set the normalized pixel blend
output_buf[output->w * j + i] = blend / (subpixel_factor * subpixel_factor);
}
}
}
}
void guidance_indi_run(bool in_flight, int32_t heading) {
//filter accel to get rid of noise
//filter attitude to synchronize with accel
guidance_indi_filter_attitude();
guidance_indi_filter_accel();
float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x; //+-ov.y/ (STABILIZATION_ATTITUDE_SP_MAX_THETA)*3.0;
float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y; //+ ov.x/ (STABILIZATION_ATTITUDE_SP_MAX_PHI)*3.0;
float speed_sp_x = pos_x_err*guidance_indi_pos_gain;
float speed_sp_y = pos_y_err*guidance_indi_pos_gain;
sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x)*guidance_indi_speed_gain;
sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y)*guidance_indi_speed_gain;
// sp_accel.x = (radio_control.values[RADIO_PITCH]/9600.0)*8.0;
// sp_accel.y = -(radio_control.values[RADIO_ROLL]/9600.0)*8.0;
// struct FloatMat33 Ga;
guidance_indi_calcG(&Ga);
MAT33_INV(Ga_inv, Ga);
float altitude_sp = POS_FLOAT_OF_BFP(guidance_v_z_sp);
float vertical_velocity_sp = guidance_indi_pos_gain*(altitude_sp - stateGetPositionNed_f()->z);
// float vertical_velocity_rc_euler = -(stabilization_cmd[COMMAND_THRUST]-4500.0)/4500.0*2.0;
float vertical_velocity_err_euler = vertical_velocity_sp - stateGetSpeedNed_f()->z;
sp_accel.z = vertical_velocity_err_euler*guidance_indi_speed_gain;
struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, 0.0};
Bound(a_diff.x, -6.0, 6.0);
Bound(a_diff.y, -6.0, 6.0);
Bound(a_diff.z, -9.0, 9.0);
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
guidance_euler_cmd.phi = roll_filt + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt + euler_cmd.y;
guidance_euler_cmd.psi = 0;//stateGetNedToBodyEulers_f()->psi;
//Bound euler angles to prevent flipping and keep upright
Bound(guidance_euler_cmd.phi, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
stabilization_attitude_set_setpoint_rp_quat_f(in_flight, heading);
}
void WardShader::Update(TimeValue t, Interval &valid) {
Point3 p, p2;
if( inUpdate )
return;
inUpdate = TRUE;
if (!ivalid.InInterval(t)) {
ivalid.SetInfinite();
// pblock->GetValue( PB_AMBIENT_CLR, t, p, ivalid );
// ambient = LimitColor(Color(p.x,p.y,p.z));
pblock->GetValue( PB_DIFFUSE_CLR, t, p, ivalid );
diffuse= LimitColor(Color(p.x,p.y,p.z));
pblock->GetValue( PB_AMBIENT_CLR, t, p2, ivalid );
if( lockAD && (p!=p2)){
pblock->SetValue( PB_AMBIENT_CLR, t, diffuse);
ambient = diffuse;
} else {
pblock->GetValue( PB_AMBIENT_CLR, t, p, ivalid );
ambient = Bound(Color(p.x,p.y,p.z));
}
pblock->GetValue( PB_SPECULAR_CLR, t, p2, ivalid );
if( lockDS && (p!=p2)){
pblock->SetValue( PB_SPECULAR_CLR, t, diffuse);
specular = diffuse;
} else {
pblock->GetValue( PB_SPECULAR_CLR, t, p, ivalid );
specular = Bound(Color(p.x,p.y,p.z));
}
// pblock->GetValue( PB_SPECULAR_CLR, t, p, ivalid );
// specular = LimitColor(Color(p.x,p.y,p.z));
pblock->GetValue( PB_GLOSSINESS_X, t, glossinessX, ivalid );
LIMITMINMAX(glossinessX, 0.0001f, 1.0f );
pblock->GetValue( PB_GLOSSINESS_Y, t, glossinessY, ivalid );
LIMITMINMAX(glossinessY, 0.0001f, 1.0f );
pblock->GetValue( PB_SPECULAR_LEV, t, specLevel, ivalid );
LIMITMINMAX(specLevel,0.0f,4.00f);
pblock->GetValue( PB_DIFFUSE_LEV, t, diffLevel, ivalid );
LIMITMINMAX(diffLevel,0.0f,2.0f);
curTime = t;
}
valid &= ivalid;
inUpdate = FALSE;
}
