static void integrate( uint8_t wp_target ) {
/* Inspired from Arnold Schroeter's code */
int i = 0;
while (bomb_z > 0. && i < MAX_STEPS) {
/* relative wind experienced by the ball (wind in NED frame) */
float airx = -bomb_vx + stateGetHorizontalWindspeed_f()->y;
float airy = -bomb_vy + stateGetHorizontalWindspeed_f()->x;
float airz = -bomb_vz;
/* alpha / m * air */
float beta = ALPHA_M * sqrt(airx*airx+airy*airy+airz*airz);
/* Euler integration */
bomb_vx += airx * beta * DT;
bomb_vy += airy * beta * DT;
bomb_vz += (airz * beta - G) * DT;
bomb_x += bomb_vx * DT;
bomb_y += bomb_vy * DT;
bomb_z += bomb_vz * DT;
i++;
}
if (bomb_z > 0.) {
/* Integration not finished -> approximation of the time with the current
speed */
float t = - bomb_z / bomb_vz;
bomb_x += bomb_vx * t;
bomb_y += bomb_vy * t;
}
waypoints[WP_RELEASE].x = waypoints[wp_target].x - bomb_x;
waypoints[WP_RELEASE].y = waypoints[wp_target].y - bomb_y;
}
void parse_mf_daq_msg(void)
{
mf_daq.nb = dl_buffer[2];
if (mf_daq.nb > 0) {
if (mf_daq.nb > MF_DAQ_SIZE) {
mf_daq.nb = MF_DAQ_SIZE;
}
// Store data struct directly from dl_buffer
float *buf = (float*)(dl_buffer+3);
memcpy(mf_daq.values, buf, mf_daq.nb * sizeof(float));
// Log on SD card
if (log_started) {
DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog,
&autopilot_flight_time,
&stateGetBodyRates_f()->p,
&stateGetBodyRates_f()->q,
&stateGetBodyRates_f()->r,
&stateGetNedToBodyEulers_f()->phi,
&stateGetNedToBodyEulers_f()->theta,
&stateGetNedToBodyEulers_f()->psi,
&stateGetAccelNed_f()->x,
&stateGetAccelNed_f()->y,
&stateGetAccelNed_f()->z,
&stateGetSpeedEnu_f()->x,
&stateGetSpeedEnu_f()->y,
&stateGetSpeedEnu_f()->z,
&stateGetPositionLla_f()->lat,
&stateGetPositionLla_f()->lon,
&stateGetPositionLla_f()->alt,
&stateGetHorizontalWindspeed_f()->y,
&stateGetHorizontalWindspeed_f()->x);
}
}
}
/*�Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
bool snav_on_time(float nominal_radius)
{
nominal_radius = fabs(nominal_radius);
float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x);
float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr));
float remaining_time = snav_desired_tow - gps.tow / 1000.;
/* Use the nominal airspeed if the estimated one is not realistic */
float airspeed = stateGetAirspeed_f();
if (airspeed < NOMINAL_AIRSPEED / 2. ||
airspeed > 2.* NOMINAL_AIRSPEED) {
airspeed = NOMINAL_AIRSPEED;
}
/* Recompute ground speeds every 10 s */
if (ground_speed_timer == 0) {
ground_speed_timer = 40; /* every 10s, called at 40Hz */
compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y,
stateGetHorizontalWindspeed_f()->x); // Wind in NED frame
}
ground_speed_timer--;
/* Time to complete the circle at nominal_radius */
float nominal_time = 0.;
float a;
float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */
/* Going one step too far */
for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) {
float qdr = current_qdr + Sign(a_radius) * a;
ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2);
nominal_time += ANGLE_STEP * nominal_radius / ground_speed;
}
/* Removing what exceeds remaining_angle */
nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed;
/* Radius size to finish in one single circle */
float radius = remaining_time / nominal_time * nominal_radius;
if (radius > 2. * nominal_radius) {
radius = nominal_radius;
}
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
radius *= Sign(a_radius);
wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
nav_circle_XY(wp_ca.x, wp_ca.y, radius);
/* Stay in this mode until the end of time */
return (remaining_time > 0);
}
void ahrs_update_gps(void) {
float hspeed_mod_f = gps.gspeed / 100.;
float course_f = gps.course / 1e7;
// Heading estimator from wind-information, usually computed with -DWIND_INFO
// wind_north and wind_east initialized to 0, so still correct if not updated
float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x;
float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y;
heading = atan2f(w_ve, w_vn);
if (heading < 0.)
heading += 2 * M_PI;
}
bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) {
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y);
waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir);
waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir);
return FALSE;
}
/** Compute a first approximation for the RELEASE waypoint from wind and
expected airspeed and altitude */
unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, float nav_drop_radius ) {
waypoints[WP_RELEASE].a = waypoints[wp_start].a;
nav_drop_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a;
nav_drop_x = 0.;
nav_drop_y = 0.;
/* We approximate vx and vy, taking into account that RELEASE is next to
TARGET */
float x_0 = waypoints[wp_target].x - waypoints[wp_start].x;
float y_0 = waypoints[wp_target].y - waypoints[wp_start].y;
/* Unit vector from START to TARGET */
float d = sqrt(x_0*x_0+y_0*y_0);
float x1 = x_0 / d;
float y_1 = y_0 / d;
waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * nav_drop_radius;
waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * nav_drop_radius;
waypoints[WP_BASELEG].a = waypoints[wp_start].a;
nav_drop_start_qdr = M_PI - atan2(-y_1, -x1);
if (nav_drop_radius < 0)
nav_drop_start_qdr += M_PI;
// wind in NED frame
if (stateIsAirspeedValid()) {
nav_drop_vx = x1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y;
nav_drop_vy = y_1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x;
}
else {
// use approximate airspeed, initially set to AIRSPEED_AT_RELEASE
nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
nav_drop_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x;
}
nav_drop_vz = 0.;
float vx0 = nav_drop_vx;
float vy0 = nav_drop_vy;
integrate(wp_target);
waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0;
waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0;
waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
return 0;
}
bool_t nav_anemotaxis_init( uint8_t c ) {
status = UTURN;
sign = 1;
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y);
waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI);
waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI);
last_plume_was_here();
return FALSE;
}
/* For a landing UPWIND.
Computes Top Of Descent waypoint from Touch Down and Approach Fix
waypoints, using glide airspeed, glide vertical speed and wind */
static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) {
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrtf( td_af_x*td_af_x + td_af_y*td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(wind->x*wind->x + wind->y*wind->y));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
return FALSE;
}
/** Compute a first approximation for the RELEASE waypoint from wind and
expected airspeed and altitude */
unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start, float bomb_radius ) {
waypoints[WP_RELEASE].a = waypoints[wp_start].a;
bomb_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a;
bomb_x = 0.;
bomb_y = 0.;
/* We approximate vx and vy, taking into account that RELEASE is next to
TARGET */
float x_0 = waypoints[wp_target].x - waypoints[wp_start].x;
float y_0 = waypoints[wp_target].y - waypoints[wp_start].y;
/* Unit vector from START to TARGET */
float d = sqrt(x_0*x_0+y_0*y_0);
float x1 = x_0 / d;
float y_1 = y_0 / d;
waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * bomb_radius;
waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * bomb_radius;
waypoints[WP_BASELEG].a = waypoints[wp_start].a;
bomb_start_qdr = M_PI - atan2(-y_1, -x1);
if (bomb_radius < 0)
bomb_start_qdr += M_PI;
// wind in NED frame
bomb_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
bomb_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x;
bomb_vz = 0.;
float vx0 = bomb_vx;
float vy0 = bomb_vy;
integrate(wp_target);
waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0;
waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0;
waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
return 0;
}
static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
{
if ((WaypointX(_af) == WaypointX(_td)) && (WaypointY(_af) == WaypointY(_td))) {
WaypointX(_af) = WaypointX(_td) - 1;
}
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt(td_af_x * td_af_x + td_af_y * td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tanf(app_angle));
// set wapoint TOD (top of decent)
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
// calculate ground speed on final (target_speed - head wind)
struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
float wind_norm = sqrt(wind->x * wind->x + wind->y * wind->y);
float wind_on_final = wind_norm * (((td_af_x * wind->y) / (td_af * wind_norm)) +
((td_af_y * wind->x) / (td_af * wind_norm)));
Bound(wind_on_final, -MAX_WIND_ON_FINAL, MAX_WIND_ON_FINAL);
gs_on_final = target_speed - wind_on_final;
// calculate position of SD (start decent)
float t_sd_intercept = (gs_on_final * tanf(app_angle)) / app_intercept_rate; //time
sd_intercept = gs_on_final * t_sd_intercept; // distance
sd_tod = 0.5 * sd_intercept;
// set waypoint SD (start decent)
WaypointX(_sd) = WaypointX(_tod) + td_af_x / td_af * sd_tod;
WaypointY(_sd) = WaypointY(_tod) + td_af_y / td_af * sd_tod;
WaypointAlt(_sd) = WaypointAlt(_af);
// calculate td_sd
float td_sd_x = WaypointX(_sd) - WaypointX(_td);
float td_sd_y = WaypointY(_sd) - WaypointY(_td);
float td_sd = sqrt(td_sd_x * td_sd_x + td_sd_y * td_sd_y);
// calculate sd_tod in x,y
sd_tod_x = WaypointX(_tod) - WaypointX(_sd);
sd_tod_y = WaypointY(_tod) - WaypointY(_sd);
// set Waypoint AF at least befor SD
if ((td_sd + app_distance_af_sd) > td_af) {
WaypointX(_af) = WaypointX(_sd) + td_af_x / td_af * app_distance_af_sd;
WaypointY(_af) = WaypointY(_sd) + td_af_y / td_af * app_distance_af_sd;
}
return FALSE;
} /* end of gls_copute_TOD */
void mf_daq_send_state(void)
{
// Send aircraft state to DAQ board
DOWNLINK_SEND_MF_DAQ_STATE(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
&autopilot_flight_time,
&stateGetBodyRates_f()->p,
&stateGetBodyRates_f()->q,
&stateGetBodyRates_f()->r,
&stateGetNedToBodyEulers_f()->phi,
&stateGetNedToBodyEulers_f()->theta,
&stateGetNedToBodyEulers_f()->psi,
&stateGetAccelNed_f()->x,
&stateGetAccelNed_f()->y,
&stateGetAccelNed_f()->z,
&stateGetSpeedEnu_f()->x,
&stateGetSpeedEnu_f()->y,
&stateGetSpeedEnu_f()->z,
&stateGetPositionLla_f()->lat,
&stateGetPositionLla_f()->lon,
&stateGetPositionLla_f()->alt,
&stateGetHorizontalWindspeed_f()->y,
&stateGetHorizontalWindspeed_f()->x);
}
bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
if (chemo_sensor) {
last_plume_was_here();
waypoints[plume].x = stateGetPositionEnu_f()->x;
waypoints[plume].y = stateGetPositionEnu_f()->y;
// DownlinkSendWp(plume);
}
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
switch (status) {
case UTURN:
NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y;
float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS);
waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
// DownlinkSendWp(c1);
// DownlinkSendWp(c2);
status = CROSSWIND;
nav_init_stage();
}
break;
case CROSSWIND:
NavSegment(c1, c2);
if (NavApproaching(c2, CARROT)) {
waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS*upwind_y;
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
nav_init_stage();
}
if (chemo_sensor) {
waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y;
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
}
chemo_sensor = 0;
return TRUE;
}
bool nav_survey_disc_run(uint8_t center_wp, float radius)
{
struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
struct FloatVect2 upwind;
upwind.x = cos(wind_dir);
upwind.y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (disc_survey.status) {
case UTURN:
nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid * disc_survey.sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) {
disc_survey.c1.x = stateGetPositionEnu_f()->x;
disc_survey.c1.y = stateGetPositionEnu_f()->y;
struct FloatVect2 dist;
VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]);
float d = VECT2_DOT_PRODUCT(upwind, dist);
if (d > radius) {
disc_survey.status = DOWNWIND;
} else {
float w = sqrtf(radius * radius - d * d) - 1.5 * grid;
struct FloatVect2 crosswind;
crosswind.x = -upwind.y;
crosswind.y = upwind.x;
disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x;
disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y;
disc_survey.status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND:
disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius;
disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius;
disc_survey.status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y);
if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) {
disc_survey.c.x = disc_survey.c2.x + grid * upwind.x;
disc_survey.c.y = disc_survey.c2.y + grid * upwind.y;
disc_survey.sign = -disc_survey.sign;
disc_survey.status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */
return true;
}
bool_t disc_survey( uint8_t center, float radius) {
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (status) {
case UTURN: Ｕ形弯模式
nav_circle_XY(c.x, c.y, grid*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
c1.x = stateGetPositionEnu_f()->x;
c1.y = stateGetPositionEnu_f()->y;
float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->y-WaypointY(center));
if (d > radius) {
status = DOWNWIND;
} else {
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND: //这是什么模式？
c2.x = WaypointX(center) - upwind_x * radius;
c2.y = WaypointY(center) - upwind_y * radius;
status = SEGMENT;
/* No break; */
case SEGMENT: //线段模式
nav_route_xy(c1.x, c1.y, c2.x, c2.y);
if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
c.x = c2.x + grid*upwind_x;
c.y = c2.y + grid*upwind_y;
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
return TRUE;
}
