/** \brief Decide if the UAV is approaching the current waypoint.
* Computes \a dist2_to_wp and compare it to square \a carrot.
* Return true if it is smaller. Else computes by scalar products if
* uav has not gone past waypoint.
* \a approaching_time can be negative and in this case, the UAV will
* fly after the waypoint for the given number of seconds.
*
* @return true if the position (x, y) is reached
*/
bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {
/** distance to waypoint in x */
float pw_x = x - stateGetPositionEnu_f()->x;
/** distance to waypoint in y */
float pw_y = y - stateGetPositionEnu_f()->y;
if (approaching_time < 0.) {
// fly after the destination waypoint
float leg_x = x - from_x;
float leg_y = y - from_y;
float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value
float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
return (scal_prod < exceed_dist);
}
else {
// fly close enough of the waypoint or cross it
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
if (dist2_to_wp < min_dist*min_dist) {
return TRUE;
}
float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
return (scal_prod < 0.);
}
}
/** Update the RELEASE location with the actual ground speed and altitude */
unit_t bomb_update_release( uint8_t wp_target ) {
bomb_z = stateGetPositionEnu_f()->z - waypoints[wp_target].a;
bomb_x = 0.;
bomb_y = 0.;
bomb_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
bomb_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
bomb_vz = 0.;
integrate(wp_target);
return 0;
}
/** Update the RELEASE location with the actual ground speed and altitude */
unit_t nav_drop_update_release( uint8_t wp_target ) {
nav_drop_z = stateGetPositionUtm_f()->alt - waypoints[wp_target].a;
nav_drop_x = 0.;
nav_drop_y = 0.;
nav_drop_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
nav_drop_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
nav_drop_vz = 0.;
integrate(wp_target);
return 0;
}
/** monitor stuff run at 1Hz */
void monitor_task(void)
{
if (autopilot.flight_time) {
autopilot.flight_time++;
}
#if defined DATALINK || defined SITL
datalink_time++;
#endif
static uint8_t t = 0;
if (ap_electrical.vsupply < CATASTROPHIC_BAT_LEVEL) {
t++;
} else {
t = 0;
}
#if !USE_GENERATED_AUTOPILOT
// only check for static autopilot
autopilot.kill_throttle |= (t >= CATASTROPHIC_BAT_KILL_DELAY);
autopilot.kill_throttle |= autopilot.launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
#endif
if (!autopilot.flight_time &&
stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
autopilot.flight_time = 1;
autopilot.launch = true; /* Not set in non auto launch */
#if DOWNLINK
uint16_t time_sec = sys_time.nb_sec;
DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
#endif
}
}
/** monitor stuff run at 1Hz */
void monitor_task( void ) {
if (autopilot_flight_time)
autopilot_flight_time++;
#if defined DATALINK || defined SITL
datalink_time++;
#endif
static uint8_t t = 0;
if (vsupply < CATASTROPHIC_BAT_LEVEL*10)
t++;
else
t = 0;
kill_throttle |= (t >= LOW_BATTERY_DELAY);
kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
if (!autopilot_flight_time &&
*stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
autopilot_flight_time = 1;
launch = TRUE; /* Not set in non auto launch */
uint16_t time_sec = sys_time.nb_sec;
DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &time_sec);
}
#ifdef USE_GPIO
GpioUpdate1();
#endif
}
void ArduIMU_periodicGPS(void)
{
if (ardu_gps_trans.status != I2CTransDone) { return; }
#if USE_HIGH_ACCEL_FLAG
// Test for high acceleration:
// - low speed
// - high thrust
float speed = stateGetHorizontalSpeedNorm_f();
if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
#endif
FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed
FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps.course); // course
ardu_gps_trans.buf[12] = gps.fix; // status gps fix
ardu_gps_trans.buf[13] = (uint8_t)arduimu_calibrate_neutrals; // calibration flag
ardu_gps_trans.buf[14] = (uint8_t)
high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
i2c_transmit(&ARDUIMU_I2C_DEV, &ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
// Reset calibration flag
if (arduimu_calibrate_neutrals) { arduimu_calibrate_neutrals = FALSE; }
}
void dc_send_shot_position(void)
{
// angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f);
// course in decideg
int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
int16_t photo_nr = -1;
if (dc_photo_nr < DC_IMAGE_BUFFER) {
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
}
/** Navigates around (x, y). Clockwise iff radius > 0 */
void nav_circle_XY(float x, float y, float radius)
{
struct EnuCoor_f *pos = stateGetPositionEnu_f();
float last_trigo_qdr = nav_circle_trigo_qdr;
nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
float sign_radius = radius > 0 ? 1 : -1;
if (nav_in_circle) {
float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
NormRadAngle(trigo_diff);
nav_circle_radians += trigo_diff;
trigo_diff *= - sign_radius;
if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius
nav_circle_radians_no_rewind += trigo_diff;
}
}
float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
float dist_carrot = CARROT * NOMINAL_AIRSPEED;
radius += -nav_shift;
float abs_radius = fabs(radius);
/** Computes a prebank. Go straight if inside or outside the circle */
circle_bank =
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atanf(stateGetHorizontalSpeedNorm_f() * stateGetHorizontalSpeedNorm_f() / (NAV_GRAVITY * radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI / 4);
carrot_angle = Max(carrot_angle, M_PI / 16);
float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
float radius_carrot = abs_radius;
if (nav_mode == NAV_MODE_COURSE) {
radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
}
fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
y + sinf(alpha_carrot)*radius_carrot);
nav_in_circle = true;
nav_circle_x = x;
nav_circle_y = y;
nav_circle_radius = radius;
}
static inline void v_ctl_set_groundspeed( void ) {
// 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_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain;
}
void nav_glide(uint8_t start_wp, uint8_t wp)
{
float start_alt = waypoints[start_wp].a;
float diff_alt = waypoints[wp].a - start_alt;
float alt = start_alt + nav_leg_progress * diff_alt;
float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
NavVerticalAltitudeMode(alt, pre_climb);
}
/** \brief Computes cruise throttle from ground speed setpoint
*/
static void nav_ground_speed_loop( void ) {
if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
&& nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
float err = nav_ground_speed_setpoint - (*stateGetHorizontalSpeedNorm_f());
v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err;
Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
} else {
/* Reset cruise throttle to nominal value */
v_ctl_auto_throttle_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
}
}
static void send_fp_min(struct transport_tx *trans, struct link_device *dev)
{
#if USE_GPS
uint16_t gspeed = gps.gspeed;
#else
// ground speed in cm/s
uint16_t gspeed = stateGetHorizontalSpeedNorm_f() / 100;
#endif
pprz_msg_send_ROTORCRAFT_FP_MIN(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&gspeed);
}
/** \brief Decide if the UAV is approaching the current waypoint.
* Computes \a dist2_to_wp and compare it to square \a carrot.
* Return true if it is smaller. Else computes by scalar products if
* uav has not gone past waypoint.
* Return true if it is the case.
*/
bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {
/** distance to waypoint in x */
float pw_x = x - stateGetPositionEnu_f()->x;
/** distance to waypoint in y */
float pw_y = y - stateGetPositionEnu_f()->y;
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
if (dist2_to_wp < min_dist*min_dist)
return TRUE;
float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
return (scal_prod < 0.);
}
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 dc_send_shot_position(void)
{
// angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
// course in decideg
int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = stateGetHorizontalSpeedNorm_f() * 10;
int16_t photo_nr = -1;
if (dc_photo_nr < DC_IMAGE_BUFFER) {
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
#if DC_SHOT_EXTRA_DL
// send a message on second datalink first
// (for instance an embedded CPU)
DOWNLINK_SEND_DC_SHOT(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
#endif
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
}
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 (kill_throttle
&& (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 (!kill_throttle) {
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 */
}
/**
* \brief
*
*/
void h_ctl_course_loop ( void ) {
static float last_err;
// Ground path error
float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f());
NormRadAngle(err);
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
+ h_ctl_course_pgain * speed_depend_nav * err
+ h_ctl_course_dgain * d_err;
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
//static inline void fly_to_xy(float x, float y) {
void fly_to_xy(float x, float y) {
struct EnuCoor_f* pos = stateGetPositionEnu_f();
desired_x = x;
desired_y = y;
if (nav_mode == NAV_MODE_COURSE) {
h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
if (h_ctl_course_setpoint < 0.)
h_ctl_course_setpoint += 2 * M_PI;
lateral_mode = LATERAL_MODE_COURSE;
} else {
float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
NormRadAngle(diff);
BoundAbs(diff,M_PI/2.);
float s = sinf(diff);
float speed = *stateGetHorizontalSpeedNorm_f();
h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
}
}
/**
* Send Metrics typically displayed on a HUD for fixed wing aircraft.
*/
static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
{
/* Current heading in degrees, in compass units (0..360, 0=north) */
int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
/* Current throttle setting in integer percent, 0 to 100 */
// is a 16bit unsigned int but supposed to be from 0 to 100??
uint16_t throttle;
#ifdef COMMAND_THRUST
throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
#elif defined COMMAND_THROTTLE
throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
#endif
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
stateGetAirspeed_f(),
stateGetHorizontalSpeedNorm_f(), // groundspeed
heading,
throttle,
stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL
stateGetSpeedNed_f()->z); // climb rate
MAVLinkSendMessage();
}
/**
* \brief
*
*/
void h_ctl_course_loop(void)
{
static float last_err;
// Ground path error
float err = stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
NormRadAngle(err);
#ifdef STRONG_WIND
// Usefull path speed
const float reference_advance = (NOMINAL_AIRSPEED / 2.);
float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;
if (
(advance < 1.) && // Path speed is small
(stateGetHorizontalSpeedNorm_f() < reference_advance) // Small path speed is due to wind (small groundspeed)
) {
/*
// rough crabangle approximation
float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
float wind_dir = atan2(wind_east,wind_north);
float wind_course = h_ctl_course_setpoint - wind_dir;
NormRadAngle(wind_course);
estimator_hspeed_dir = estimator_psi;
float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
//crab = estimator_hspeed_mod - estimator_psi;
NormRadAngle(crab);
*/
// Heading error
float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
NormRadAngle(herr);
if (advance < -0.5) { //<! moving in the wrong direction / big > 90 degree turn
err = herr;
} else if (advance < 0.) { //<!
err = (-advance) * 2. * herr;
} else {
err = advance * err;
}
// Reset differentiator when switching mode
//if (h_ctl_course_heading_mode == 0)
// last_err = err;
//h_ctl_course_heading_mode = 1;
}
/* else
{
// Reset differentiator when switching mode
if (h_ctl_course_heading_mode == 1)
last_err = err;
h_ctl_course_heading_mode = 0;
}
*/
#endif //STRONG_WIND
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
#ifdef H_CTL_COURSE_SLEW_INCREMENT
/* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
static float h_ctl_course_slew_rate = 0.;
float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */
float half_nav_angle_saturation = nav_angle_saturation / 2.;
if (autopilot.launch) { /* prevent accumulator run-up on the ground */
if (err > half_nav_angle_saturation) {
h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));
h_ctl_course_slew_rate += h_ctl_course_slew_increment;
} else if (err < -half_nav_angle_saturation) {
h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate));
h_ctl_course_slew_rate -= h_ctl_course_slew_increment;
} else {
h_ctl_course_slew_rate = 0.;
}
}
#endif
float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
#if defined(AGR_CLIMB) && !USE_AIRSPEED
/** limit navigation during extreme altitude changes */
if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||
v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_BLENDED) {
BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
/* altitude: z-up is positive -> positive error -> too low */
if (v_ctl_altitude_error > 0) {
nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
(AGR_BLEND_START - AGR_BLEND_END));
Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
} else {
nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
(AGR_BLEND_START - AGR_BLEND_END));
Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
}
cmd *= nav_ratio;
}
}
#endif
float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;
#ifdef H_CTL_ROLL_SLEW
float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
BoundAbs(diff_roll, h_ctl_roll_slew);
h_ctl_roll_setpoint += diff_roll;
#else
h_ctl_roll_setpoint = roll_setpoint;
#endif
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
bool nav_bungee_takeoff_run(void)
{
float cross = 0.;
// Get current position
struct FloatVect2 pos;
VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);
switch (CTakeoffStatus) {
case Launch:
// Recalculate lines if below min speed
if (stateGetHorizontalSpeedNorm_f() < BUNGEE_TAKEOFF_MIN_SPEED) {
compute_points_from_bungee();
}
// Follow Launch Line with takeoff pitch and no throttle
NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
NavVerticalThrottleMode(0);
// FIXME previously using altitude mode, maybe not wise without motors
//NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.);
nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);
kill_throttle = 1;
// Find out if UAV has crossed the line
VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point
cross = VECT2_DOT_PRODUCT(pos, takeoff_dir);
if (cross > 0. && stateGetHorizontalSpeedNorm_f() > BUNGEE_TAKEOFF_MIN_SPEED) {
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
} else {
// If not crossed stay in this status
break;
}
// Start throttle imidiatelly
case Throttle:
//Follow Launch Line
NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE));
nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);
kill_throttle = 0;
if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT)
#if USE_AIRSPEED
&& (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED)
#endif
) {
CTakeoffStatus = Finished;
return false;
} else {
return true;
}
break;
default:
// Invalid status or Finished, end function
return false;
}
return true;
}
int formation_flight(void)
{
static uint8_t _1Hz = 0;
uint8_t nb = 0, i;
float hspeed_dir = stateGetHorizontalSpeedDir_f();
float ch = cosf(hspeed_dir);
float sh = sinf(hspeed_dir);
form_n = 0.;
form_e = 0.;
form_a = 0.;
form_speed = stateGetHorizontalSpeedNorm_f();
form_speed_n = form_speed * ch;
form_speed_e = form_speed * sh;
if (AC_ID == leader_id) {
stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
}
// set info for this AC
set_ac_info(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir,
stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow);
// broadcast info
uint8_t ac_id = AC_ID;
uint8_t status = formation[the_acs_id[AC_ID]].status;
DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
if (++_1Hz >= 4) {
_1Hz = 0;
DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode,
&formation[the_acs_id[AC_ID]].east,
&formation[the_acs_id[AC_ID]].north,
&formation[the_acs_id[AC_ID]].alt);
}
if (formation[the_acs_id[AC_ID]].status != ACTIVE) { return FALSE; } // AC not ready
// get leader info
struct ac_info_ * leader = get_ac_info(leader_id);
if (formation[the_acs_id[leader_id]].status == UNSET ||
formation[the_acs_id[leader_id]].status == IDLE) {
// leader not ready or not in formation
return FALSE;
}
// compute slots in the right reference frame
struct slot_ form[NB_ACS];
float cr = 0., sr = 1.;
if (form_mode == FORM_MODE_COURSE) {
cr = cosf(leader->course);
sr = sinf(leader->course);
}
for (i = 0; i < NB_ACS; ++i) {
if (formation[i].status == UNSET) { continue; }
form[i].east = formation[i].east * sr - formation[i].north * cr;
form[i].north = formation[i].east * cr + formation[i].north * sr;
form[i].alt = formation[i].alt;
}
// compute control forces
for (i = 0; i < NB_ACS; ++i) {
if (the_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
if (delta_t > FORM_CARROT) {
// if AC not responding for too long
formation[i].status = LOST;
continue;
} else {
// compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
formation[i].status = ACTIVE;
if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) {
form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x)
- (form[i].east - form[the_acs_id[AC_ID]].east);
form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y)
- (form[i].north - form[the_acs_id[AC_ID]].north);
form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
form_speed += ac->gspeed;
//form_speed_e += ac->gspeed * sinf(ac->course);
//form_speed_n += ac->gspeed * cosf(ac->course);
++nb;
}
}
}
uint8_t _nb = Max(1, nb);
form_n /= _nb;
form_e /= _nb;
form_a /= _nb;
form_speed = form_speed / (nb + 1) - stateGetHorizontalSpeedNorm_f();
//form_speed_e = form_speed_e / (nb+1) - stateGetHorizontalSpeedNorm_f() * sh;
//form_speed_n = form_speed_n / (nb+1) - stateGetHorizontalSpeedNorm_f() * ch;
// set commands
NavVerticalAutoThrottleMode(0.);
// altitude loop
float alt = 0.;
if (AC_ID == leader_id) {
alt = nav_altitude;
} else {
alt = leader->alt - form[the_acs_id[leader_id]].alt;
}
alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT);
// carrot
if (AC_ID != leader_id) {
float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east;
float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north;
desired_x = leader->east + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx;
desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy;
// fly to desired
fly_to_xy(desired_x, desired_y);
desired_x = leader->east + dx;
desired_y = leader->north + dy;
// lateral correction
//float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
//float diff_course = leader->course - hspeed_dir;
//NormRadAngle(diff_course);
//h_ctl_roll_setpoint += coef_form_course * diff_course;
//h_ctl_roll_setpoint += coef_form_course * diff_heading;
}
//BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
// speed loop
if (nb > 0) {
float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
v_ctl_auto_throttle_cruise_throttle = cruise;
}
return TRUE;
}
bool_t nav_bungee_takeoff_run(void)
{
//Translate current position so Throttle point is (0,0)
float Currentx = stateGetPositionEnu_f()->x-throttlePx;
float Currenty = stateGetPositionEnu_f()->y-throttlePy;
bool_t CurrentAboveLine;
float ThrottleB;
switch(CTakeoffStatus)
{
case Launch:
//Follow Launch Line
NavVerticalAutoThrottleMode(0);
NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.);
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 1;
//recalculate lines if below min speed
if((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed)
{
initialx = stateGetPositionEnu_f()->x;
initialy = stateGetPositionEnu_f()->y;
//Translate initial position so that the position of the bungee is (0,0)
Currentx = initialx-(WaypointX(BungeeWaypoint));
Currenty = initialy-(WaypointY(BungeeWaypoint));
//Find Launch line slope
float MLaunch = Currenty/Currentx;
//Find Throttle Point (the point where the throttle line and launch line intersect)
if(Currentx < 0)
throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
else
throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
if(Currenty < 0)
throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
else
throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
//Find ThrottleLine
ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
//Determine whether the UAV is below or above the throttle line
if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
AboveLine = TRUE;
else
AboveLine = FALSE;
//Translate the throttle point back
throttlePx = throttlePx+(WaypointX(BungeeWaypoint));
throttlePy = throttlePy+(WaypointY(BungeeWaypoint));
}
//Find out if the UAV is currently above the line
if(Currenty > (ThrottleSlope*Currentx))
CurrentAboveLine = TRUE;
else
CurrentAboveLine = FALSE;
//Find out if UAV has crossed the line
if(AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed)
{
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
}
break;
case Throttle:
//Follow Launch Line
NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
NavVerticalThrottleMode(9600*(1));
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 0;
if((stateGetPositionUtm_f()->alt > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed))
{
CTakeoffStatus = Finished;
return FALSE;
}
else
{
return TRUE;
}
break;
default:
break;
}
return TRUE;
}
/**
* 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);
}
bool_t gls_run(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
{
// set target speed for approach on final
if (init) {
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed;
#endif
init = FALSE;
}
// calculate distance tod_td
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.);
struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
float hspeed = *stateGetHorizontalSpeedNorm_f();
float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
(pos_enu->y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress, -1, 1);
// float nav_final_length = sqrt(final2);
// calculate requiered decent rate on glideslope
float pre_climb_glideslope = hspeed * (-tanf(app_angle));
// calculate glideslope
float start_alt = WaypointAlt(_tod);
float diff_alt = WaypointAlt(_td) - start_alt;
float alt_glideslope = start_alt + nav_final_progress * diff_alt;
// calculate intercept
float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2 * sd_tod_x +
(pos_enu->y - WaypointY(_sd)) * 2 * sd_tod_y) /
Max((sd_intercept * sd_intercept), 1.);
Bound(nav_intercept_progress, -1, 1);
float tmp = nav_intercept_progress * sd_intercept / gs_on_final;
float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp);
float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle));
//########################################################
// handle the different vertical approach segments
float pre_climb = 0.;
float alt = 0.;
// distance
float f_af = sqrt((pos_enu->x - WaypointX(_af)) * (pos_enu->x - WaypointX(_af)) +
(pos_enu->y - WaypointY(_af)) * (pos_enu->y - WaypointY(_af)));
if (f_af < app_distance_af_sd) { // approach fix (AF) to start descent (SD)
alt = WaypointAlt(_af);
pre_climb = 0.;
} else if ((f_af > app_distance_af_sd) && (f_af < (app_distance_af_sd + sd_intercept))) {
// start descent (SD) to intercept
alt = alt_intercept;
pre_climb = pre_climb_intercept;
} else { //glideslope (intercept to touch down)
alt = alt_glideslope;
pre_climb = pre_climb_glideslope;
}
// Bound(pre_climb, -5, 0.);
//######################### autopilot modes
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()
// GENERIC TRAJECTORY CONTROLLER
void gvf_control_2D(float ke, float kn, float e,
struct gvf_grad *grad, struct gvf_Hess *hess)
{
struct FloatEulers *att = stateGetNedToBodyEulers_f();
float ground_speed = stateGetHorizontalSpeedNorm_f();
float course = stateGetHorizontalSpeedDir_f();
float px_dot = ground_speed * sinf(course);
float py_dot = ground_speed * cosf(course);
int s = gvf_control.s;
// gradient Phi
float nx = grad->nx;
float ny = grad->ny;
// tangent to Phi
float tx = s * grad->ny;
float ty = -s * grad->nx;
// Hessian
float H11 = hess->H11;
float H12 = hess->H12;
float H21 = hess->H21;
float H22 = hess->H22;
// Calculation of the desired angular velocity in the vector field
float pdx_dot = tx - ke * e * nx;
float pdy_dot = ty - ke * e * ny;
float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
float md_x = pdx_dot / norm_pd_dot;
float md_y = pdy_dot / norm_pd_dot;
float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
+ ((-ke * e * H12) + s * H22) * py_dot;
float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
- (s * H12 + (ke * e * H22)) * py_dot;
float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
/ norm_pd_dot;
float md_dot_x = md_y * md_dot_const;
float md_dot_y = -md_x * md_dot_const;
float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
float mr_x = sinf(course);
float mr_y = cosf(course);
float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
// Coordinated turn
if (autopilot_get_mode() == AP_MODE_AUTO2) {
h_ctl_roll_setpoint =
-atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
}
}
int potential_task(void)
{
uint8_t i;
float ch = cosf(stateGetHorizontalSpeedDir_f());
float sh = sinf(stateGetHorizontalSpeedDir_f());
potential_force.east = 0.;
potential_force.north = 0.;
potential_force.alt = 0.;
// compute control forces
int8_t nb = 0;
for (i = 0; i < NB_ACS; ++i) {
if (the_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
// if AC not responding for too long, continue, else compute force
if (delta_t > CARROT) { continue; }
else {
float sha = sinf(ac->course);
float cha = cosf(ac->course);
float de = ac->east + sha * delta_t - stateGetPositionEnu_f()->x;
if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; }
float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y;
if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; }
float da = ac->alt + ac->climb * delta_t - stateGetPositionUtm_f()->alt;
if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; }
float dist = sqrtf(de * de + dn * dn + da * da);
if (dist == 0.) { continue; }
float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha;
float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha;
float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float scal = dve * de + dvn * dn + dva * da;
if (scal < 0.) { continue; } // No risk of collision
float d3 = dist * dist * dist;
potential_force.east += scal * de / d3;
potential_force.north += scal * dn / d3;
potential_force.alt += scal * da / d3;
++nb;
}
}
if (nb == 0) { return true; }
//potential_force.east /= nb;
//potential_force.north /= nb;
//potential_force.alt /= nb;
// set commands
NavVerticalAutoThrottleMode(0.);
// carrot
float dx = -force_pos_gain * potential_force.east;
float dy = -force_pos_gain * potential_force.north;
desired_x += dx;
desired_y += dy;
// fly to desired
fly_to_xy(desired_x, desired_y);
// speed loop
float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
cruise += -force_speed_gain * (potential_force.north * ch + potential_force.east * sh);
Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
potential_force.speed = cruise;
v_ctl_auto_throttle_cruise_throttle = cruise;
// climb loop
potential_force.climb = -force_climb_gain * potential_force.alt;
BoundAbs(potential_force.climb, V_CTL_ALTITUDE_MAX_CLIMB);
NavVerticalClimbMode(potential_force.climb);
DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice, &potential_force.east, &potential_force.north,
&potential_force.alt, &potential_force.speed, &potential_force.climb);
return true;
}
/* conflicts detection and monitoring */
void tcas_periodic_task_1Hz( void ) {
// no TCAS under security_height
if (stateGetPositionEnu_f()->z < GROUND_ALT + SECURITY_HEIGHT) {
uint8_t i;
for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
return;
}
// test possible conflicts
float tau_min = tcas_tau_ta;
uint8_t ac_id_close = AC_ID;
uint8_t i;
float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f()));
float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f()));
for (i = 2; i < NB_ACS; i++) {
if (the_acs[i].ac_id == 0) continue; // no AC data
uint32_t dt = gps.tow - the_acs[i].itow;
if (dt > 3*TCAS_DT_MAX) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
continue;
}
if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
float dx = the_acs[i].east - stateGetPositionEnu_f()->x;
float dy = the_acs[i].north - stateGetPositionEnu_f()->y;
float dz = the_acs[i].alt - stateGetPositionEnu_f()->z;
float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float scal = dvx*dx + dvy*dy + dvz*dz;
float ddh = dx*dx + dy*dy;
float ddv = dz*dz;
float tau = TCAS_HUGE_TAU;
if (scal > 0.) tau = (ddh + ddv) / scal;
// monitor conflicts
uint8_t inside = TCAS_IsInside();
//enum tcas_resolve test_dir = RA_NONE;
switch (tcas_acs_status[i].status) {
case TCAS_RA:
if (tau >= TCAS_HUGE_TAU && !inside) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
tcas_acs_status[i].resolve = RA_NONE;
DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
}
break;
case TCAS_TA:
if (tau < tcas_tau_ra || inside) {
tcas_acs_status[i].status = TCAS_RA; // TA -> RA
// Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
break;
}
if (tau > tcas_tau_ta && !inside)
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
tcas_acs_status[i].resolve = RA_NONE;
DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
break;
case TCAS_NO_ALARM:
if (tau < tcas_tau_ta || inside) {
tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
// Downlink warning
DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
}
if (tau < tcas_tau_ra || inside) {
tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
// Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);
}
break;
}
// store closest AC
if (tau < tau_min) {
tau_min = tau;
ac_id_close = the_acs[i].ac_id;
}
}
// set current conflict mode
if (tcas_status == TCAS_RA && tcas_ac_RA != AC_ID && tcas_acs_status[the_acs_id[tcas_ac_RA]].status == TCAS_RA) {
ac_id_close = tcas_ac_RA; // keep RA until resolved
}
tcas_status = tcas_acs_status[the_acs_id[ac_id_close]].status;
// at least one in conflict, deal with closest one
if (tcas_status == TCAS_RA) {
tcas_ac_RA = ac_id_close;
tcas_resolve = tcas_test_direction(tcas_ac_RA);
uint8_t ac_resolve = tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve;
if (ac_resolve != RA_NONE) { // first resolution, no message received
if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
else tcas_resolve = RA_CLIMB;
}
tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now
}
else { // second resolution or message received
if (ac_resolve != RA_LEVEL) { // message received
if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
else tcas_resolve = RA_CLIMB;
}
}
else { // no message
if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) tcas_resolve = RA_DESCEND; // revert resolve
else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // revert resolve
}
}
// Downlink alert
DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&tcas_ac_RA,&tcas_resolve);
}
else tcas_ac_RA = AC_ID; // no conflict
#ifdef TCAS_DEBUG
if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice,&ac_id_close,&tau_min);
#endif
}
bool nav_launcher_run(void)
{
//Find distance from laucher
float dist_x = stateGetPositionEnu_f()->x - launch_x;
float dist_y = stateGetPositionEnu_f()->y - launch_y;
float launch_dist = sqrtf(dist_x * dist_x + dist_y * dist_y);
if (launch_dist <= 0.01) {
launch_dist = 0.01;
}
switch (CLaunch_Status) {
case L_Pitch_Nav:
//Follow Launch Line
NavVerticalAltitudeMode(launch_alt, 0);
NavVerticalAutoThrottleMode(LAUNCHER_TAKEOFF_PITCH);
NavVerticalThrottleMode(MAX_PPRZ * (1));
NavAttitude(0);
kill_throttle = 0;
//If the plane has been launched and has traveled for more than a specified distance, switch to line nav
if (stateGetHorizontalSpeedNorm_f() > LAUNCHER_TAKEOFF_MIN_SPEED_LINE) {
if (launch_dist > LAUNCHER_TAKEOFF_DISTANCE) {
launch_line_x = stateGetPositionEnu_f()->x;
launch_line_y = stateGetPositionEnu_f()->y;
CLaunch_Status = L_Line_Nav;
}
}
break;
case L_Line_Nav:
//Follow Launch Line
NavVerticalAltitudeMode(launch_alt, 0);
NavVerticalAutoThrottleMode(LAUNCHER_TAKEOFF_PITCH);
NavVerticalThrottleMode(MAX_PPRZ * (1));
nav_route_xy(launch_x, launch_y, launch_line_x, launch_line_y);
kill_throttle = 0;
//If the aircraft is above a specific alt, greater than a specific speed or too far away, circle up
if (((stateGetPositionUtm_f()->alt
> (launch_alt - LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD))
&& (stateGetHorizontalSpeedNorm_f()
> LAUNCHER_TAKEOFF_MIN_SPEED_CIRCLE))
|| (launch_dist > LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE)) {
CLaunch_Status = L_CircleUp;
//Find position of circle
float x_1 = dist_x / launch_dist;
float y_1 = dist_y / launch_dist;
launch_circle.x = stateGetPositionEnu_f()->x
+ y_1 * LAUNCHER_TAKEOFF_CIRCLE_RADIUS;
launch_circle.y = stateGetPositionEnu_f()->y
- x_1 * LAUNCHER_TAKEOFF_CIRCLE_RADIUS;
}
break;
case L_CircleUp:
NavVerticalAutoThrottleMode(0);
NavVerticalAltitudeMode(launch_circle_alt, 0);
nav_circle_XY(launch_circle.x, launch_circle.y,
LAUNCHER_TAKEOFF_CIRCLE_RADIUS);
if (stateGetPositionUtm_f()->alt
> (launch_circle_alt - LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD)) {
CLaunch_Status = L_Finished;
return FALSE;
}
break;
default:
break;
}
return TRUE;
}
