/// 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_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); nav_spiral.radius = float_vect2_norm(&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; }
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; }
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; } }