bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
{
VECT2_COPY(nav_spiral.center, waypoints[center_wp]); // center of the helix
nav_spiral.center.z = waypoints[center_wp].a;
nav_spiral.radius_start = radius_start; // start radius of the helix
nav_spiral.segments = segments;
nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS;
if (nav_spiral.radius_start < nav_spiral.radius_min)
nav_spiral.radius_start = nav_spiral.radius_min;
nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral
struct FloatVect2 edge;
VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
FLOAT_VECT2_NORM(nav_spiral.radius, edge);
// get a copy of the current position
struct EnuCoor_f pos_enu;
memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
// nav_spiral.alpha_limit denotes angle, where the radius will be increased
nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments;
//current position
nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;
nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
if(nav_spiral.dist_from_center > nav_spiral.radius)
nav_spiral.status = SpiralOutside;
return FALSE;
}
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
{
struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
VECT2_DIFF(wp_diff, *wp_end, *wp_start);
VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
// go back to metric precision or values are too large
VECT2_COPY(wp_diff_prec, wp_diff);
INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_POS_FRAC);
uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
nav_leg_length = int32_sqrt(leg_length2);
nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
nav_leg_progress += progress;
int32_t prog_2 = nav_leg_length;
Bound(nav_leg_progress, 0, prog_2);
struct Int32Vect2 progress_pos;
VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
VECT2_SUM(navigation_target, *wp_start, progress_pos);
nav_segment_start = *wp_start;
nav_segment_end = *wp_end;
horizontal_mode = HORIZONTAL_MODE_ROUTE;
dist2_to_wp = get_dist2_to_point(wp_end);
}
/// Utility function: converts lla (int) to local point (float)
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
{
// return FALSE if there is no valid local coordinate system
if (!state.ned_initialized_i) {
return FALSE;
}
// change geoid alt to ellipsoid alt
lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
//Compute ENU components from LLA with respect to ltp origin
struct EnuCoor_i tmp_enu_point_i;
enu_of_lla_point_i(&tmp_enu_point_i, &state.ned_origin_i, lla);
struct EnuCoor_f tmp_enu_point_f;
ENU_FLOAT_OF_BFP(tmp_enu_point_f, tmp_enu_point_i);
//Bound the new waypoint with max distance from home
struct EnuCoor_f home;
ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]);
struct FloatVect2 vect_from_home;
VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
//Saturate the mission wp not to overflow max_dist_from_home
//including a buffer zone before limits
float dist_to_home = float_vect2_norm(&vect_from_home);
dist_to_home += BUFFER_ZONE_DIST;
if (dist_to_home > MAX_DIST_FROM_HOME) {
VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
}
// set new point
VECT2_SUM(*point, home, vect_from_home);
point->z = tmp_enu_point_f.z;
return TRUE;
}
bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
{
uint16_t time_at_wp;
uint32_t dist_to_point;
static uint16_t wp_entry_time = 0;
static bool_t wp_reached = FALSE;
static struct EnuCoor_i wp_last = { 0, 0, 0 };
struct Int32Vect2 diff;
if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
wp_reached = FALSE;
wp_last = *wp;
}
VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
dist_to_point = int32_vect2_norm(&diff);
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
if (!wp_reached) {
wp_reached = TRUE;
wp_entry_time = autopilot_flight_time;
time_at_wp = 0;
} else {
time_at_wp = autopilot_flight_time - wp_entry_time;
}
} else {
time_at_wp = 0;
wp_reached = FALSE;
}
if (time_at_wp > stay_time) {
INT_VECT3_ZERO(wp_last);
return TRUE;
}
return FALSE;
}
bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
{
int32_t dist_to_point;
struct Int32Vect2 diff;
struct EnuCoor_i *pos = stateGetPositionEnu_i();
/* if an approaching_time is given, estimate diff after approching_time secs */
if (approaching_time > 0) {
struct Int32Vect2 estimated_pos;
struct Int32Vect2 estimated_progress;
struct EnuCoor_i *speed = stateGetSpeedEnu_i();
VECT2_SMUL(estimated_progress, *speed, approaching_time);
INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
VECT2_SUM(estimated_pos, *pos, estimated_progress);
VECT2_DIFF(diff, *wp, estimated_pos);
}
/* else use current position */
else {
VECT2_DIFF(diff, *wp, *pos);
}
/* compute distance of estimated/current pos to target wp
* distance with half metric precision (6.25 cm)
*/
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
dist_to_point = int32_vect2_norm(&diff);
/* return TRUE if we have arrived */
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
return TRUE;
}
/* if coming from a valid waypoint */
if (from != NULL) {
/* return TRUE if normal line at the end of the segment is crossed */
struct Int32Vect2 from_diff;
VECT2_DIFF(from_diff, *wp, *from);
INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2);
return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
}
return FALSE;
}
static void compute_points_from_bungee(void)
{
// Store init point (current position, where the plane will be released)
VECT2_ASSIGN(init_point, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);
// Compute unitary 2D vector (bungee_point - init_point) = takeoff direction
VECT2_DIFF(takeoff_dir, bungee_point, init_point);
float_vect2_normalize(&takeoff_dir);
// Find throttle point (the point where the throttle line and launch line intersect)
// If TakeOff_Distance is positive, throttle point is after bungee point, before otherwise
VECT2_SMUL(throttle_point, takeoff_dir, BUNGEE_TAKEOFF_DISTANCE);
VECT2_SUM(throttle_point, bungee_point, throttle_point);
}
void rotorcraft_cam_periodic(void)
{
switch (rotorcraft_cam_mode) {
case ROTORCRAFT_CAM_MODE_NONE:
#if ROTORCRAFT_CAM_USE_TILT
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
#endif
#if ROTORCRAFT_CAM_USE_PAN
rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
#endif
break;
case ROTORCRAFT_CAM_MODE_MANUAL:
// nothing to do here, just apply tilt pwm at the end
break;
case ROTORCRAFT_CAM_MODE_HEADING:
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
#if ROTORCRAFT_CAM_USE_PAN
INT32_COURSE_NORMALIZE(rotorcraft_cam_pan);
nav_heading = rotorcraft_cam_pan;
#endif
break;
case ROTORCRAFT_CAM_MODE_WP:
#ifdef ROTORCRAFT_CAM_TRACK_WP
{
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
rotorcraft_cam_pan = int32_atan2(diff.x, diff.y);
nav_heading = rotorcraft_cam_pan;
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
int32_t dist, height;
dist = INT32_VECT2_NORM(diff);
height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
rotorcraft_cam_tilt = int32_atan2(height, dist);
Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
}
#endif
break;
default:
break;
}
#if ROTORCRAFT_CAM_USE_TILT
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
#endif
}
void booz_cam_periodic(void) {
switch (booz_cam_mode) {
case BOOZ_CAM_MODE_NONE:
#ifdef BOOZ_CAM_USE_TILT
booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
#endif
#ifdef BOOZ_CAM_USE_PAN
booz_cam_pan = ahrs.ltp_to_body_euler.psi;
#endif
break;
case BOOZ_CAM_MODE_MANUAL:
#ifdef BOOZ_CAM_USE_TILT
Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
#endif
break;
case BOOZ_CAM_MODE_HEADING:
#ifdef BOOZ_CAM_USE_TILT_ANGLES
Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX);
booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
#endif
#ifdef BOOZ_CAM_USE_PAN
Bound(booz_cam_pan, CP_MIN, CP_MAX);
nav_heading = booz_cam_pan;
#endif
break;
case BOOZ_CAM_MODE_WP:
#ifdef WP_CAM
{
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos);
INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
nav_heading = booz_cam_pan;
#ifdef BOOZ_CAM_USE_TILT_ANGLES
int32_t dist, height;
INT32_VECT2_NORM(dist, diff);
height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC;
INT32_ATAN2(booz_cam_tilt, height, dist);
Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
#endif
}
#endif
break;
}
#ifdef BOOZ_CAM_USE_TILT
BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
#endif
}
void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) {
if (radius == 0) {
VECT2_COPY(navigation_target, *wp_center);
dist2_to_wp = get_dist2_to_point(wp_center);
}
else {
struct Int32Vect2 pos_diff;
VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
// go back to half metric precision or values are too large
//INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
// store last qdr
int32_t last_qdr = nav_circle_qdr;
// compute qdr
nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x);
// increment circle radians
if (nav_circle_radians != 0) {
int32_t angle_diff = nav_circle_qdr - last_qdr;
INT32_ANGLE_NORMALIZE(angle_diff);
nav_circle_radians += angle_diff;
}
else {
// Smallest angle to increment at next step
nav_circle_radians = 1;
}
// direction of rotation
int8_t sign_radius = radius > 0 ? 1 : -1;
// absolute radius
int32_t abs_radius = abs(radius);
// carrot_angle
int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
int32_t s_carrot, c_carrot;
PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
PPRZ_ITRIG_COS(c_carrot, carrot_angle);
// compute setpoint
VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
VECT2_SUM(navigation_target, *wp_center, pos_diff);
}
nav_circle_center = *wp_center;
nav_circle_radius = radius;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
}
static inline void UNUSED nav_advance_carrot(void)
{
struct EnuCoor_i *pos = stateGetPositionEnu_i();
/* compute a vector to the waypoint */
struct Int32Vect2 path_to_waypoint;
VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
/* saturate it */
VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15));
int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
VECT2_COPY(navigation_carrot, navigation_target);
} else {
struct Int32Vect2 path_to_carrot;
VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
}
}
bool nav_spiral_run(void)
{
struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();
VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
float DistanceStartEstim;
float CircleAlpha;
switch (nav_spiral.status) {
case SpiralOutside:
//flys until center of the helix is reached an start helix
nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
// center reached?
if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) {
// nadir image
#ifdef DIGITAL_CAM
dc_send_command(DC_SHOOT);
#endif
nav_spiral.status = SpiralStartCircle;
}
break;
case SpiralStartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to SpiralCircle
nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start);
if (nav_spiral.dist_from_center >= nav_spiral.radius_start) {
VECT2_COPY(nav_spiral.last_circle, pos_enu);
nav_spiral.status = SpiralCircle;
// Start helix
#ifdef DIGITAL_CAM
dc_Circle(360 / nav_spiral.segments);
#endif
}
break;
case SpiralCircle: {
nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
// equation:
// alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
DistanceStartEstim = float_vect2_norm(&pos_diff);
CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start)));
if (CircleAlpha >= nav_spiral.alpha_limit) {
VECT2_COPY(nav_spiral.last_circle, pos_enu);
nav_spiral.status = SpiralInc;
}
break;
}
case SpiralInc:
// increasing circle radius as long as it is smaller than max helix radius
if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) {
nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment;
#ifdef DIGITAL_CAM
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI;
}
#endif
} else {
nav_spiral.radius_start = nav_spiral.radius;
#ifdef DIGITAL_CAM
// Stopps DC
dc_stop();
#endif
}
nav_spiral.status = SpiralCircle;
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */
return true;
}
/**
* main navigation routine. This is called periodically evaluates the current
* Position and stage and navigates accordingly.
* @returns True until the survey is finished
*/
bool nav_survey_polygon_run(void)
{
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
//entry circle around entry-center until the desired altitude is reached
if (survey.stage == ENTRY) {
nav_circle_XY(survey.entry_center.x, survey.entry_center.y, -survey.psa_min_rad);
if (NavCourseCloseTo(survey.segment_angle)
&& nav_approaching_xy(survey.seg_start.x, survey.seg_start.y, last_x, last_y, CARROT)
&& fabs(stateGetPositionUtm_f()->alt - survey.psa_altitude) <= 20) {
survey.stage = SEG;
nav_init_stage();
#ifdef DIGITAL_CAM
dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
#endif
}
}
//fly the segment until seg_end is reached
if (survey.stage == SEG) {
nav_points(survey.seg_start, survey.seg_end);
//calculate all needed points for the next flyover
if (nav_approaching_xy(survey.seg_end.x, survey.seg_end.y, survey.seg_start.x, survey.seg_start.y, 0)) {
#ifdef DIGITAL_CAM
dc_stop();
#endif
VECT2_DIFF(survey.seg_center1, survey.seg_end, survey.rad_vec);
survey.ret_start.x = survey.seg_end.x - 2 * survey.rad_vec.x;
survey.ret_start.y = survey.seg_end.y - 2 * survey.rad_vec.y;
//if we get no intersection the survey is finished
static struct FloatVect2 sum_start_sweep;
static struct FloatVect2 sum_end_sweep;
VECT2_SUM(sum_start_sweep, survey.seg_start, survey.sweep_vec);
VECT2_SUM(sum_end_sweep, survey.seg_end, survey.sweep_vec);
if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) {
return false;
}
survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2 * survey.rad_vec.x;
survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2 * survey.rad_vec.y;
survey.seg_center2.x = survey.seg_start.x - 0.5 * (2.0 * survey.rad_vec.x + survey.sweep_vec.x);
survey.seg_center2.y = survey.seg_start.y - 0.5 * (2.0 * survey.rad_vec.y + survey.sweep_vec.y);
survey.stage = TURN1;
nav_init_stage();
}
}
//turn from stage to return
else if (survey.stage == TURN1) {
nav_circle_XY(survey.seg_center1.x, survey.seg_center1.y, -survey.psa_min_rad);
if (NavCourseCloseTo(survey.return_angle)) {
survey.stage = RET;
nav_init_stage();
}
//return
} else if (survey.stage == RET) {
nav_points(survey.ret_start, survey.ret_end);
if (nav_approaching_xy(survey.ret_end.x, survey.ret_end.y, survey.ret_start.x, survey.ret_start.y, 0)) {
survey.stage = TURN2;
nav_init_stage();
}
//turn from return to stage
} else if (survey.stage == TURN2) {
nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2 * survey.psa_min_rad + survey.psa_sweep_width) * 0.5);
if (NavCourseCloseTo(survey.segment_angle)) {
survey.stage = SEG;
nav_init_stage();
#ifdef DIGITAL_CAM
dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
#endif
}
}
return true;
}
/**
* initializes the variables needed for the survey to start
* @param first_wp the first Waypoint of the polygon
* @param size the number of points that make up the polygon
* @param angle angle in which to do the flyovers
* @param sweep_width distance between the sweeps
* @param shot_dist distance between the shots
* @param min_rad minimal radius when navigating
* @param altitude the altitude that must be reached before the flyover starts
**/
void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude)
{
int i;
struct FloatVect2 small, sweep;
float divider, angle_rad = angle / 180.0 * M_PI;
if (angle < 0.0) {
angle += 360.0;
}
if (angle >= 360.0) {
angle -= 360.0;
}
survey.poly_first = first_wp;
survey.poly_count = size;
survey.psa_sweep_width = sweep_width;
survey.psa_min_rad = min_rad;
survey.psa_shot_dist = shot_dist;
survey.psa_altitude = altitude;
survey.segment_angle = angle;
survey.return_angle = angle + 180;
if (survey.return_angle > 359) {
survey.return_angle -= 360;
}
if (angle <= 45.0 || angle >= 315.0) {
//north
survey.dir_vec.y = 1.0;
survey.dir_vec.x = 1.0 * tanf(angle_rad);
sweep.x = 1.0;
sweep.y = - survey.dir_vec.x / survey.dir_vec.y;
} else if (angle <= 135.0) {
//east
survey.dir_vec.x = 1.0;
survey.dir_vec.y = 1.0 / tanf(angle_rad);
sweep.y = - 1.0;
sweep.x = survey.dir_vec.y / survey.dir_vec.x;
} else if (angle <= 225.0) {
//south
survey.dir_vec.y = -1.0;
survey.dir_vec.x = -1.0 * tanf(angle_rad);
sweep.x = -1.0;
sweep.y = survey.dir_vec.x / survey.dir_vec.y;
} else {
//west
survey.dir_vec.x = -1.0;
survey.dir_vec.y = -1.0 / tanf(angle_rad);
sweep.y = 1.0;
sweep.x = - survey.dir_vec.y / survey.dir_vec.x;
}
//normalize
FLOAT_VECT2_NORMALIZE(sweep);
VECT2_SMUL(survey.rad_vec, sweep, survey.psa_min_rad);
VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width);
//begin at leftmost position (relative to survey.dir_vec)
VECT2_COPY(small, waypoints[survey.poly_first]);
divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y);
//calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (divider < 0.0) {
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
} else
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
//calculate the line the defines the first flyover
survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x;
survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y;
VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec);
if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) {
survey.stage = ERR;
return;
}
//center of the entry circle
VECT2_DIFF(survey.entry_center, survey.seg_start, survey.rad_vec);
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
survey.stage = ENTRY;
}
/**
* initializes the variables needed for the survey to start.
*
* @param center_wp the waypoint defining the center of the survey-rectangle
* @param dir_wp the waypoint defining the orientation of the survey-rectangle
* @param sweep_length the length of the survey-rectangle
* @param sweep_spacing distance between the sweeps
* @param sweep_lines number of sweep_lines to fly
* @param altitude the altitude that must be reached before the flyover starts
*/
bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
{
zs.current_laps = 0;
zs.pre_leave_angle = 2;
// copy variables from the flight plan
VECT2_COPY(zs.wp_center, waypoints[center_wp]);
VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
zs.altitude = altitude;
// if turning right leave circle before angle is reached, if turning left - leave after
if (sweep_spacing > 0) {
zs.pre_leave_angle -= zs.pre_leave_angle;
}
struct FloatVect2 flight_vec;
VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
FLOAT_VECT2_NORMALIZE(flight_vec);
// calculate the flight_angle
zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
zs.return_angle = zs.flight_angle + 180;
if (zs.return_angle > 359) {
zs.return_angle -= 360;
}
// calculate the vector from one flightline perpendicular to the next flightline left,
// seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
// (used later to move the flight pattern one flightline up for each round)
zs.sweep_width.x = -flight_vec.y * sweep_spacing;
zs.sweep_width.y = +flight_vec.x * sweep_spacing;
// calculate number of laps to fly and turning radius for each end
zs.total_laps = (sweep_lines+1)/2;
zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;
struct FloatVect2 flight_line;
VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
//CALCULATE THE NAVIGATION POINTS
//start and end of flight-line in flight-direction
VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
struct FloatVect2 sweep_span;
VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
//start and end of flight-line in return-direction
VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
//turn-centers at both ends
zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(100.0);
NavVerticalAltitudeMode(zs.altitude, 0.0);
zs.stage = Z_ENTRY;
return FALSE;
}
void dc_periodic_4Hz(void)
{
static uint8_t dc_shutter_timer = 0;
switch (dc_autoshoot) {
case DC_AUTOSHOOT_PERIODIC:
if (dc_shutter_timer) {
dc_shutter_timer--;
} else {
dc_shutter_timer = dc_autoshoot_quartersec_period;
dc_send_command(DC_SHOOT);
}
break;
case DC_AUTOSHOOT_DISTANCE:
{
struct FloatVect2 cur_pos;
cur_pos.x = stateGetPositionEnu_f()->x;
cur_pos.y = stateGetPositionEnu_f()->y;
struct FloatVect2 delta_pos;
VECT2_DIFF(delta_pos, cur_pos, last_shot_pos);
float dist_to_last_shot = float_vect2_norm(&delta_pos);
if (dist_to_last_shot > dc_distance_interval) {
dc_gps_count++;
dc_send_command(DC_SHOOT);
VECT2_COPY(last_shot_pos, cur_pos);
}
}
break;
case DC_AUTOSHOOT_CIRCLE:
{
float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
if (course < 0.)
course += 360.;
float current_block = floorf(course/dc_circle_interval);
if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
dc_gps_count++;
dc_circle_last_block = current_block;
dc_send_command(DC_SHOOT);
}
}
break;
case DC_AUTOSHOOT_SURVEY:
{
float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;
if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) {
dc_gps_next_dist += dc_survey_interval;
dc_gps_count++;
dc_send_command(DC_SHOOT);
}
}
break;
default:
dc_autoshoot = DC_AUTOSHOOT_STOP;
}
}
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
/* WARNING: SPEED SATURATION UNTESTED */
VECT2_ADD(gh_pos_ref, gh_speed_ref);
VECT2_ADD(gh_speed_ref, gh_accel_ref);
// compute speed error
struct Int32Vect2 speed_err;
INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
VECT2_DIFF(speed_err, gh_speed_ref, speed_err);
// convert to accel resolution
INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
// compute accel from speed_sp
VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU);
INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC);
/* Compute route reference before saturation */
// use metric precision or values are too large
INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x);
/* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum speed*/
gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
/* restore gh_speed_ref range (Q14.17) */
INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
/* Saturate accelerations */
if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
gh_accel_ref.x = -gh_max_accel_ref.x;
}
else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
gh_accel_ref.x = gh_max_accel_ref.x;
}
if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
gh_accel_ref.y = -gh_max_accel_ref.y;
}
else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
gh_accel_ref.y = gh_max_accel_ref.y;
}
/* Saturate speed and adjust acceleration accordingly */
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
gh_speed_ref.x = -gh_max_speed_ref.x;
if (gh_accel_ref.x < 0)
gh_accel_ref.x = 0;
}
else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
gh_speed_ref.x = gh_max_speed_ref.x;
if (gh_accel_ref.x > 0)
gh_accel_ref.x = 0;
}
if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
gh_speed_ref.y = -gh_max_speed_ref.y;
if (gh_accel_ref.y < 0)
gh_accel_ref.y = 0;
}
else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
gh_speed_ref.y = gh_max_speed_ref.y;
if (gh_accel_ref.y > 0)
gh_accel_ref.y = 0;
}
}
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
VECT2_ADD(gh_pos_ref, gh_speed_ref);
VECT2_ADD(gh_speed_ref, gh_accel_ref);
// compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
struct Int32Vect2 speed;
INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA);
INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
// compute pos error in pos_sp resolution
struct Int32Vect2 pos_err;
INT32_VECT2_RSHIFT(pos_err, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC));
VECT2_DIFF(pos_err, pos_err, pos_sp);
// convert to accel resolution
INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
// compute the "pos part" of accel
struct Int32Vect2 pos;
VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2));
INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC);
// sum accel
VECT2_SUM(gh_accel_ref, speed, pos);
/* Compute route reference before saturation */
// use metric precision or values are too large
INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x);
/* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum speed*/
gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
/* restore gh_speed_ref range (Q14.17) */
INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
/* Saturate accelerations */
if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
gh_accel_ref.x = -gh_max_accel_ref.x;
}
else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
gh_accel_ref.x = gh_max_accel_ref.x;
}
if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
gh_accel_ref.y = -gh_max_accel_ref.y;
}
else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
gh_accel_ref.y = gh_max_accel_ref.y;
}
/* Saturate speed and adjust acceleration accordingly */
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
gh_speed_ref.x = -gh_max_speed_ref.x;
if (gh_accel_ref.x < 0)
gh_accel_ref.x = 0;
}
else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
gh_speed_ref.x = gh_max_speed_ref.x;
if (gh_accel_ref.x > 0)
gh_accel_ref.x = 0;
}
if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
gh_speed_ref.y = -gh_max_speed_ref.y;
if (gh_accel_ref.y < 0)
gh_accel_ref.y = 0;
}
else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
gh_speed_ref.y = gh_max_speed_ref.y;
if (gh_accel_ref.y > 0)
gh_accel_ref.y = 0;
}
}
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 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;
}
