/** set waypoint to current horizontal location without modifying altitude */
void waypoint_set_here_2d(uint8_t wp_id)
{
if (wp_id >= nb_waypoint) {
return;
}
if (waypoint_is_global(wp_id)) {
waypoint_set_latlon(wp_id, stateGetPositionLla_i());
} else {
waypoint_set_xy_i(wp_id, stateGetPositionEnu_i()->x, stateGetPositionEnu_i()->y);
}
}
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
}
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);
}
void nav_init_stage(void)
{
VECT3_COPY(nav_last_point, *stateGetPositionEnu_i());
stage_time = 0;
nav_circle_radians = 0;
//horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
}
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;
}
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);
}
// Called on each vision analysis result after receiving the struct
void run_avoid_navigation_onvision(void)
{
// Send ALL vision data to the ground
DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 5, avoid_navigation_data.stereo_bin);
switch (avoid_navigation_data.mode) {
case 0: // Go to Goal and stop at obstacles
//count 4 subsequent obstacles
if (avoid_navigation_data.stereo_bin[0] > 1) {
counter = counter + 1;
if (counter > 1) {
counter = 0;
//Obstacle detected, go to turn until clear mode
obstacle_detected = TRUE;
avoid_navigation_data.mode = 1;
}
} else {
counter = 0;
}
break;
case 1: // Turn until clear
//count 20 subsequent free frames
if (avoid_navigation_data.stereo_bin[0] < 1) {
counter = counter + 1;
if (counter > 12) {
counter = 0;
//Stop and put waypoint 2.5 m ahead
struct EnuCoor_i new_coor;
struct EnuCoor_i *pos = stateGetPositionEnu_i();
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH));
new_coor.z = pos->z;
waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y);
obstacle_detected = FALSE;
avoid_navigation_data.mode = 0;
}
} else {
counter = 0;
}
break;
case 2:
break;
default: // do nothing
break;
}
avoid_navigation_data.stereo_bin[2] = avoid_navigation_data.stereo_bin[0] > 20;
avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode;
avoid_navigation_data.stereo_bin[4] = counter;
#ifdef STEREO_LED
if (obstacle_detected) {
LED_ON(STEREO_LED);
} else {
LED_OFF(STEREO_LED);
}
#endif
}
static void send_fp(void) {
int32_t carrot_up = -guidance_v_z_sp;
DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&guidance_h_pos_sp.y,
&guidance_h_pos_sp.x,
&carrot_up,
&guidance_h_heading_sp,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
}
static void send_fp(struct transport_tx *trans, struct link_device *dev)
{
int32_t carrot_up = -guidance_v_z_sp;
pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&guidance_h.sp.pos.y,
&guidance_h.sp.pos.x,
&carrot_up,
&guidance_h.sp.heading,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
}
/** set waypoint to current location and altitude */
void waypoint_set_here(uint8_t wp_id)
{
if (wp_id >= nb_waypoint) {
return;
}
if (waypoint_is_global(wp_id)) {
waypoint_set_lla(wp_id, stateGetPositionLla_i());
} else {
waypoint_set_enu_i(wp_id, stateGetPositionEnu_i());
}
}
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;
}
uint8_t moveWaypointForwards(uint8_t waypoint, float distanceMeters)
{
struct EnuCoor_i new_coor;
struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position
// Calculate the sine and cosine of the heading the drone is keeping
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
// Now determine where to place the waypoint you want to go to
new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (distanceMeters));
new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (distanceMeters));
new_coor.z = pos->z; // Keep the height the same
// Set the waypoint to the calculated position
waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y);
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 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);
}
}
uint8_t emma69(uint8_t waypoint) {
float wp1_x = -1.0;
float wp1_y = 1.0;
float h1 = 0.0;
float wp2_x = -1.0;
float wp2_y = -1.0;
float h2 = 0.0;
float wp3_x = 1.0;
float wp3_y = 1.0;
float h3 = 0.0;
float wp4_x = 1.0;
float wp4_y = -1.0;
float h4 = 0.0;
float dist_threshold = 0.1;
double wps[8] = {wp1_x, wp1_y, wp2_x, wp2_y, wp3_x, wp3_y, wp4_x, wp4_y};
double headings[4] = {h1,h2,h3,h4};
struct EnuCoor_i new_coor;
struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position
//struct EnuCoor_f *pos = stateGetPositionEnu_f(); // Get your current position
printf("Current pos \t");
printf("posX= %f \t", POS_FLOAT_OF_BFP(pos->x)); //POS_FLOAT_OF_BFP(pos->x)
printf("posY= %f \t", POS_FLOAT_OF_BFP(pos->y)); //POS_FLOAT_OF_BFP(pos->y)
printf("\n");
float wpX = waypoint_get_x(waypoint);
float wpY = waypoint_get_y(waypoint);
printf("Current wp \t");
printf("wpX: %f \t", wpX);
printf("wpY: %f \t", wpY);
printf("\n");
//float dist_curr = POS_FLOAT_OF_BFP((POS_BFP_OF_REAL(wpX) - pos->x)*(POS_BFP_OF_REAL(wpX) - pos->x) + (POS_BFP_OF_REAL(wpY) - pos->y)*(POS_BFP_OF_REAL(wpY) - pos->y)); // POS_BFP_OF_REAL POS_FLOAT_OF_BFP(pos->x)
float dist_curr = (wpX - POS_FLOAT_OF_BFP(pos->x))*(wpX - POS_FLOAT_OF_BFP(pos->x)) + (wpY - POS_FLOAT_OF_BFP(pos->y))*(wpY - POS_FLOAT_OF_BFP(pos->y)); // POS_BFP_OF_REAL POS_FLOAT_OF_BFP(pos->x)
printf("Dist to current wp \t");
printf("dist_curr: %f \t", dist_curr);
printf("\n");
//float dist1 = (wpX - wp1_x)*(wpX - wp1_x) + (wpY - wp1_y)*(wpY - wp1_y); // Dist between current wp and navigation wps
//float dist2 = (wpX - wp2_x)*(wpX - wp2_x) + (wpY - wp2_y)*(wpY - wp2_y);
//float dist3 = (wpX - wp3_x)*(wpX - wp3_x) + (wpY - wp3_y)*(wpY - wp3_y);
//if (dist1 < dist2 && dist1 < dist3){i=1;}
//else if (dist2 < dist1 && dist2 < dist3){i=2;}
//else {i=3;}
if (dist_curr < dist_threshold*dist_threshold){
i = i + 1;
if (i> 4){i=1;}
}
// Set the waypoint to the calculated position
printf("Set waypoint to \t");
printf("i: %d \t", i);
printf("wpsX: %f \t",wps[(i-1)*2]);
printf("wpsY: %f \t",wps[(i-1)*2+1]);
printf("\n");
//struct image_t *img = v4l2_image_get(bebop_front_camera.dev, &img);
//struct a *img = v4l2_image_get(bebop_front_camera.dev, &img);
//struct image_t *img;
//printf("image height: %f \t", image_t.h);
printf("wouter is: %d", color_count);
new_coor.x = POS_BFP_OF_REAL(wps[(i-1)*2]);
new_coor.y = POS_BFP_OF_REAL(wps[(i-1)*2+1]);
new_coor.z = pos->z;
// Set the waypoint to the calculated position
waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y);
// Set heading to requested
nav_set_heading_deg(headings[i-1]);
printf("\n");
printf("\n");
return FALSE;
}
void nav_init_stage(void)
{
VECT3_COPY(nav_last_point, *stateGetPositionEnu_i());
stage_time = 0;
nav_circle_radians = 0;
}
