/** * \brief Failsafe navigation without position estimation * * Just set attitude and throttle to FAILSAFE values * to prevent the plane from crashing. */ void nav_without_gps(void) { lateral_mode = LATERAL_MODE_ROLL; v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; #ifdef SECTION_FAILSAFE h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL; nav_pitch = FAILSAFE_DEFAULT_PITCH; nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE)*MAX_PPRZ); #else h_ctl_roll_setpoint = 0; nav_pitch = 0; nav_throttle_setpoint = TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE)*MAX_PPRZ); #endif }
inline static void v_ctl_climb_auto_throttle_loop(void) { static float last_err; float f_throttle = 0; float err = estimator_z_dot - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; float controlled_throttle = v_ctl_auto_throttle_cruise_throttle + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + v_ctl_auto_throttle_pgain * (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err + v_ctl_auto_throttle_dgain * d_err); /* pitch pre-command */ float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain; #if defined AGR_CLIMB switch (v_ctl_auto_throttle_submode) { case V_CTL_AUTO_THROTTLE_AGRESSIVE: if (v_ctl_climb_setpoint > 0) { /* Climbing */ f_throttle = AGR_CLIMB_THROTTLE; nav_pitch = AGR_CLIMB_PITCH; } else { /* Going down */ f_throttle = AGR_DESCENT_THROTTLE; nav_pitch = AGR_DESCENT_PITCH; } break; case V_CTL_AUTO_THROTTLE_BLENDED: { float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END); f_throttle = (1-ratio) * controlled_throttle; nav_pitch = (1-ratio) * v_ctl_pitch_of_vz; v_ctl_auto_throttle_sum_err += (1-ratio) * err; BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); if (v_ctl_altitude_error < 0) { f_throttle += ratio * AGR_CLIMB_THROTTLE; nav_pitch += ratio * AGR_CLIMB_PITCH; } else { f_throttle += ratio * AGR_DESCENT_THROTTLE; nav_pitch += ratio * AGR_DESCENT_PITCH; } break; } case V_CTL_AUTO_THROTTLE_STANDARD: default: #endif f_throttle = controlled_throttle; v_ctl_auto_throttle_sum_err += err; BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); nav_pitch += v_ctl_pitch_of_vz; #if defined AGR_CLIMB break; } /* switch submode */ #endif v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); }
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); }
/** \brief Computes desired_gaz and desired_pitch from desired_climb */ void climb_pid_run ( void ) { float err = estimator_z_dot - desired_climb; float fgaz; if (auto_pitch) { /* gaz constant */ desired_gaz = nav_desired_gaz; desired_pitch = climb_pitch_pgain * (err + climb_pitch_igain * climb_pitch_sum_err); if (desired_pitch > max_pitch) desired_pitch = max_pitch; if (desired_pitch < min_pitch) desired_pitch = min_pitch; climb_pitch_sum_err += err; if (climb_pitch_sum_err > MAX_PITCH_CLIMB_SUM_ERR) climb_pitch_sum_err = MAX_PITCH_CLIMB_SUM_ERR; if (climb_pitch_sum_err < - MAX_PITCH_CLIMB_SUM_ERR) climb_pitch_sum_err = - MAX_PITCH_CLIMB_SUM_ERR; } else { /* pitch almost constant */ /* pitch offset for climb */ pitch_of_vz = (desired_climb > 0) ? desired_climb * pitch_of_vz_pgain : 0.; fgaz = climb_pgain * (err + climb_igain * climb_sum_err) + CLIMB_LEVEL_GAZ + CLIMB_GAZ_OF_CLIMB*desired_climb; climb_sum_err += err; if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err = MAX_CLIMB_SUM_ERR; if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = - MAX_CLIMB_SUM_ERR; desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ); desired_pitch = nav_pitch + pitch_of_vz; } }
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); }
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 = estimator_z_dot - 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 - estimator_hspeed_mod); 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 - estimator_airspeed); 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; nav_pitch = v_ctl_pitch_of_vz; v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH); }
void climb_control_task(void) { if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { if (vertical_mode >= VERTICAL_MODE_AUTO_CLIMB) { /* climb_pid_run(); function inlined below */ /* inlined climb_pid_run begins */ float err = estimator_z_dot - desired_climb; float fgaz; if (auto_pitch) { /* gaz constant */ desired_gaz = nav_desired_gaz; desired_pitch = climb_pitch_pgain * (err + climb_pitch_igain * climb_pitch_sum_err); if (desired_pitch > max_pitch) desired_pitch = max_pitch; if (desired_pitch < min_pitch) desired_pitch = min_pitch; climb_pitch_sum_err += err; if (climb_pitch_sum_err > MAX_PITCH_CLIMB_SUM_ERR) climb_pitch_sum_err = MAX_PITCH_CLIMB_SUM_ERR; if (climb_pitch_sum_err < - MAX_PITCH_CLIMB_SUM_ERR) climb_pitch_sum_err = - MAX_PITCH_CLIMB_SUM_ERR; } else { /* pitch almost constant */ /* pitch offset for climb */ pitch_of_vz = (desired_climb > 0) ? desired_climb * pitch_of_vz_pgain : 0.; fgaz = climb_pgain * (err + climb_igain * climb_sum_err) + CLIMB_LEVEL_GAZ + CLIMB_GAZ_OF_CLIMB*desired_climb; climb_sum_err += err; if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err = MAX_CLIMB_SUM_ERR; if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = - MAX_CLIMB_SUM_ERR; desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ); desired_pitch = nav_pitch + pitch_of_vz; } /* inlined climb_pid_run ends */ } if (vertical_mode == VERTICAL_MODE_AUTO_GAZ) desired_gaz = nav_desired_gaz; if (low_battery || (!estimator_flight_time && !launch)) desired_gaz = 0.; } }
/** * Auto-throttle inner loop * \brief */ void v_ctl_climb_loop(void) { // Airspeed setpoint rate limiter: // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1 float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew; BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude); v_ctl_auto_airspeed_setpoint_slew += airspeed_incr; #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT // 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_slew) { v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew; // reset integrator of ground speed loop v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain * v_ctl_auto_groundspeed_igain); } #else v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew; #endif // Airspeed outerloop: positive means we need to accelerate float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f(); // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f; BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration); // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration #ifndef SITL struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta); #else float vdot = 0; #endif // Acceleration Error: positive means UAV needs to accelerate: needs extra energy float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot); // Flight Path Outerloop: positive means needs to climb more: needs extra energy float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled; // Total Energy Error: positive means energy should be added float en_tot_err = gamma_err + vdot_err; // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up float en_dis_err = gamma_err - vdot_err; // Auto Cruise Throttle if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) { v_ctl_auto_throttle_nominal_cruise_throttle += v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude + en_tot_err * v_ctl_energy_total_igain * dt_attidude; Bound(v_ctl_auto_throttle_nominal_cruise_throttle, 0.0f, 1.0f); } // Total Controller float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + v_ctl_auto_throttle_of_airspeed_pgain * speed_error + v_ctl_energy_total_pgain * en_tot_err; if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle == 1)) { // If your energy supply is not sufficient, then neglect the climb requirement en_dis_err = -vdot_err; // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; } if (v_ctl_climb_setpoint < 0) { v_ctl_climb_setpoint += 30. * dt_attidude; } } /* pitch pre-command */ if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) { v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude + v_ctl_energy_diff_igain * en_dis_err * dt_attidude; Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); } float v_ctl_pitch_of_vz = + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain - v_ctl_auto_pitch_of_airspeed_pgain * speed_error + v_ctl_auto_pitch_of_airspeed_dgain * vdot + v_ctl_energy_diff_pgain * en_dis_err + v_ctl_auto_throttle_nominal_cruise_pitch; if (kill_throttle) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; } v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch; Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT) ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration); v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ); }
/** \fn void navigation_task( void ) * \brief Compute desired_course */ static void navigation_task( void ) { #if defined FAILSAFE_DELAY_WITHOUT_GPS /** This section is used for the failsafe of GPS */ static uint8_t last_pprz_mode; /** If aircraft is launched and is in autonomus mode, go into PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */ if (launch) { if (GpsTimeoutError) { if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { last_pprz_mode = pprz_mode; pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; PERIODIC_SEND_PPRZ_MODE(DefaultChannel); gps_lost = TRUE; } } else /* GPS is ok */ if (gps_lost) { /** If aircraft was in failsafe mode, come back in previous mode */ pprz_mode = last_pprz_mode; gps_lost = FALSE; PERIODIC_SEND_PPRZ_MODE(DefaultChannel); } } #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */ common_nav_periodic_task_4Hz(); if (pprz_mode == PPRZ_MODE_HOME) nav_home(); else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) nav_without_gps(); else nav_periodic_task(); #ifdef TCAS CallTCAS(); #endif #ifndef PERIOD_NAVIGATION_DefaultChannel_0 // If not sent periodically (in default 0 mode) SEND_NAVIGATION(DefaultChannel); #endif SEND_CAM(DefaultChannel); /* The nav task computes only nav_altitude. However, we are interested by desired_altitude (= nav_alt+alt_shift) in any case. So we always run the altitude control loop */ if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) v_ctl_altitude_loop(); if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) { #ifdef H_CTL_RATE_LOOP /* Be sure to be in attitude mode, not roll */ h_ctl_auto1_rate = FALSE; #endif if (lateral_mode >=LATERAL_MODE_COURSE) h_ctl_course_loop(); /* aka compute nav_desired_roll */ if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) v_ctl_climb_loop(); if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) v_ctl_throttle_setpoint = nav_throttle_setpoint; #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 = nav_pitch; 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; } energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours) }