void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
{
nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
survey_orientation = so;
if (survey_orientation == NS) {
survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west + grid / 2.),
nav_survey_east - grid / 2.);
if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south
&& stateGetHorizontalSpeedDir_f() > M_PI / 2. && stateGetHorizontalSpeedDir_f() < 3 * M_PI / 2)) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else {
survey_from.y = nav_survey_south;
survey_to.y = nav_survey_north;
}
} else { /* survey_orientation == WE */
survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south + grid / 2.),
nav_survey_north - grid / 2.);
if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west
&& stateGetHorizontalSpeedDir_f() > M_PI)) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
} else {
survey_from.x = nav_survey_west;
survey_to.x = nav_survey_east;
}
}
nav_survey_shift = grid;
survey_uturn = FALSE;
LINE_START_FUNCTION;
}
bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {
if (chemo_sensor > last_plume_value) {
/* Move the circle in this direction */
float x = stateGetPositionEnu_f()->x - waypoints[plume].x;
float y = stateGetPositionEnu_f()->y - waypoints[plume].y;
waypoints[c].x = waypoints[plume].x + ALPHA * x;
waypoints[c].y = waypoints[plume].y + ALPHA * y;
// DownlinkSendWp(c);
/* Turn in the right direction */
float dir_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float pvect = dir_x*y - dir_y*x;
sign = (pvect > 0 ? -1 : 1);
/* Reduce the radius */
radius = sign * (DEFAULT_CIRCLE_RADIUS+(MAX_CHEMO-chemo_sensor)/(float)MAX_CHEMO*(MAX_RADIUS-DEFAULT_CIRCLE_RADIUS));
/* Store this plume */
waypoints[plume].x = stateGetPositionEnu_f()->x;
waypoints[plume].y = stateGetPositionEnu_f()->y;
// DownlinkSendWp(plume);
last_plume_value = chemo_sensor;
}
NavCircleWaypoint(c, radius);
return TRUE;
}
/** 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;
}
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);
}
/** Computes the right angles from target_x, target_y, target_alt */
void cam_target( void ) {
#ifdef TEST_CAM
vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
cam_target_x, cam_target_y, cam_target_alt,
&cam_pan_c, &cam_tilt_c);
#else
struct EnuCoor_f* pos = stateGetPositionEnu_f();
struct FloatEulers* att = stateGetNedToBodyEulers_f();
vPoint(pos->x, pos->y, pos->z,
att->phi, att->theta, *stateGetHorizontalSpeedDir_f(),
cam_target_x, cam_target_y, cam_target_alt,
&cam_pan_c, &cam_tilt_c);
#endif
cam_angles();
}
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);
}
//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;
}
}
/**
* \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);
}
/**
* \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);
}
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;
}
void airborne_ant_point_periodic(void)
{
float airborne_ant_pan_servo = 0;
static VECTOR svPlanePosition,
Home_Position,
Home_PositionForPlane,
Home_PositionForPlane2;
static MATRIX smRotation;
svPlanePosition.fx = stateGetPositionEnu_f()->y;
svPlanePosition.fy = stateGetPositionEnu_f()->x;
svPlanePosition.fz = stateGetPositionUtm_f()->alt;
Home_Position.fx = WaypointY(WP_HOME);
Home_Position.fy = WaypointX(WP_HOME);
Home_Position.fz = waypoints[WP_HOME].a;
/* distance between plane and object */
vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
/* yaw */
smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
/*
* This is for one axis pan antenna mechanisms. The default is to
* circle clockwise so view is right. The pan servo neutral makes
* the antenna look to the right with 0� given, 90� is to the back and
* -90� is to the front.
*
*
*
* plane front
*
* 90
^
* I
* 135 I 45�
* \ I /
* \I/
* 180-------I------- 0�
* /I\
* / I \
* -135 I -45�
* I
* -90
* plane back
*
*
*/
/* fPan = 0 -> antenna looks along the wing
90 -> antenna looks in flight direction
-90 -> antenna looks backwards
*/
/* fixed to the plane*/
airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
// I need to avoid oscillations around the 180 degree mark.
if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; }
if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; }
if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
airborne_ant_pan = RadOfDeg(-180);
} else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
airborne_ant_pan = RadOfDeg(180);
ant_pan_positive = 0;
}
#ifdef ANT_PAN_NEUTRAL
airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
if (airborne_ant_pan > 0) {
airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
} else {
airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
}
#endif
airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);
#ifdef COMMAND_ANT_PAN
ap_state->commands[COMMAND_ANT_PAN] = airborne_ant_pan_servo;
#endif
return;
}
/* 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
}
/* D is the current position */
bool snav_init(uint8_t a, float desired_course_rad, float radius)
{
wp_a = a;
radius = fabs(radius);
float da_x = WaypointX(wp_a) - stateGetPositionEnu_f()->x;
float da_y = WaypointY(wp_a) - stateGetPositionEnu_f()->y;
/* D_CD orthogonal to current course, CD on the side of A */
float u_x = cos(M_PI_2 - stateGetHorizontalSpeedDir_f());
float u_y = sin(M_PI_2 - stateGetHorizontalSpeedDir_f());
d_radius = - Sign(u_x * da_y - u_y * da_x) * radius;
wp_cd.x = stateGetPositionEnu_f()->x + d_radius * u_y;
wp_cd.y = stateGetPositionEnu_f()->y - d_radius * u_x;
wp_cd.a = WaypointAlt(wp_a);
/* A_CA orthogonal to desired course, CA on the side of D */
float desired_u_x = cos(M_PI_2 - desired_course_rad);
float desired_u_y = sin(M_PI_2 - desired_course_rad);
a_radius = Sign(desired_u_x * da_y - desired_u_y * da_x) * radius;
u_a_ca_x = desired_u_y;
u_a_ca_y = - desired_u_x;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
wp_ca.a = WaypointAlt(wp_a);
/* Unit vector along CD-CA */
u_x = wp_ca.x - wp_cd.x;
u_y = wp_ca.y - wp_cd.y;
float cd_ca = sqrt(u_x * u_x + u_y * u_y);
/* If it is too close in reverse direction, set CA on the other side */
if (a_radius * d_radius < 0 && cd_ca < 2 * radius) {
a_radius = -a_radius;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
u_x = wp_ca.x - wp_cd.x;
u_y = wp_ca.y - wp_cd.y;
cd_ca = sqrt(u_x * u_x + u_y * u_y);
}
u_x /= cd_ca;
u_y /= cd_ca;
if (a_radius * d_radius > 0) {
/* Both arcs are in the same direction */
/* CD_TD orthogonal to CD_CA */
wp_td.x = wp_cd.x - d_radius * u_y;
wp_td.y = wp_cd.y + d_radius * u_x;
/* CA_TA also orthogonal to CD_CA */
wp_ta.x = wp_ca.x - a_radius * u_y;
wp_ta.y = wp_ca.y + a_radius * u_x;
} else {
/* Arcs are in reverse directions: trigonemetric puzzle :-) */
float alpha = atan2(u_y, u_x) + acos(d_radius / (cd_ca / 2));
wp_td.x = wp_cd.x + d_radius * cos(alpha);
wp_td.y = wp_cd.y + d_radius * sin(alpha);
wp_ta.x = wp_ca.x + a_radius * cos(alpha);
wp_ta.y = wp_ca.y + a_radius * sin(alpha);
}
qdr_td = M_PI_2 - atan2(wp_td.y - wp_cd.y, wp_td.x - wp_cd.x);
qdr_a = M_PI_2 - atan2(WaypointY(wp_a) - wp_ca.y, WaypointX(wp_a) - wp_ca.x);
wp_td.a = wp_cd.a;
wp_ta.a = wp_ca.a;
ground_speed_timer = 0;
return false;
}
// 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;
}