/**
* \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)
}
