void v_ctl_landing_loop(void)
{
#if CTRL_VERTICAL_LANDING
static float land_speed_i_err;
static float land_alt_i_err;
static float kill_alt;
float land_speed_err = v_ctl_landing_desired_speed - stateGetHorizontalSpeedNorm_f();
float land_alt_err = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
if (autopilot_throttle_killed()
&& (kill_alt - v_ctl_altitude_setpoint)
> (v_ctl_landing_alt_throttle_kill - v_ctl_landing_alt_flare)) {
v_ctl_throttle_setpoint = 0.0; // Throttle is already in KILL (command redundancy)
nav_pitch = v_ctl_landing_pitch_flare; // desired final flare pitch
lateral_mode = LATERAL_MODE_ROLL; //override course correction during flare - eliminate possibility of catching wing tip due to course correction
h_ctl_roll_setpoint = 0.0; // command zero roll during flare
} else {
// set throttle based on altitude error
v_ctl_throttle_setpoint = land_alt_err * v_ctl_landing_throttle_pgain
+ land_alt_i_err * v_ctl_landing_throttle_igain;
Bound(v_ctl_throttle_setpoint, 0, v_ctl_landing_throttle_max * MAX_PPRZ); // cut off throttle cmd at specified MAX
land_alt_i_err += land_alt_err / CONTROL_FREQUENCY; // integrator land_alt_err, divide by control frequency
BoundAbs(land_alt_i_err, 50); // integrator sat limits
// set pitch based on ground speed error
nav_pitch -= land_speed_err * v_ctl_landing_pitch_pgain / 1000
+ land_speed_i_err * v_ctl_landing_pitch_igain / 1000; // 1000 is a multiplier to get more reasonable gains for ctl_basic
Bound(nav_pitch, -v_ctl_landing_pitch_limits, v_ctl_landing_pitch_limits); //max pitch authority for landing
land_speed_i_err += land_speed_err / CONTROL_FREQUENCY; // integrator land_speed_err, divide by control frequency
BoundAbs(land_speed_i_err, .2); // integrator sat limits
// update kill_alt until final kill throttle is initiated - allows for mode switch to first part of if statement above
// eliminates the need for knowing the altitude of TD
if (!autopilot_throttle_killed()) {
kill_alt = v_ctl_altitude_setpoint; //
if (land_alt_err > 0.0) {
nav_pitch = 0.01; // if below desired alt close to ground command level pitch
}
}
}
#endif /* CTRL_VERTICAL_LANDING */
}
/**
* 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
/* convert last imu accel measurement to float */
struct FloatVect3 accel_imu_f;
ACCELS_FLOAT_OF_BFP(accel_imu_f, accel_imu_meas);
/* rotate from imu to body frame */
struct FloatVect3 accel_meas_body;
float_quat_vmult(&accel_meas_body, &imu_to_body_quat, &accel_imu_f);
float vdot = 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 (autopilot.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) || (autopilot_throttle_killed() == 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 (autopilot.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 (autopilot_throttle_killed()) { 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);
}
/**
* Send a heartbeat
*/
static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mav_state = MAV_STATE_CALIBRATING;
uint8_t mav_mode = 0;
#ifdef AP
uint8_t mav_type = MAV_TYPE_FIXED_WING;
switch (autopilot_get_mode()) {
case PPRZ_MODE_MANUAL:
mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case PPRZ_MODE_AUTO1:
mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case PPRZ_MODE_AUTO2:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case PPRZ_MODE_HOME:
mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
break;
default:
break;
}
#else
uint8_t mav_type = MAV_TYPE_QUADROTOR;
switch (autopilot_get_mode()) {
case AP_MODE_HOME:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_RC_DIRECT:
mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_ATTITUDE_RC_CLIMB:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
case AP_MODE_CARE_FREE_DIRECT:
mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case AP_MODE_NAV:
mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
break;
case AP_MODE_GUIDED:
mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
default:
break;
}
#endif
if (stateIsAttitudeValid()) {
if (autopilot_throttle_killed()) {
mav_state = MAV_STATE_STANDBY;
} else {
mav_state = MAV_STATE_ACTIVE;
mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
}
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
mav_type,
MAV_AUTOPILOT_PPZ,
mav_mode,
0, // custom_mode
mav_state);
MAVLinkSendMessage();
}
