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

}