自学教程：C++ Bound函数代码示例

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