void atmega_i2c_cam_ctrl_periodic(void)
{
dc_periodic_4Hz();
// Request Status
dc_send_command(DC_GET_STATUS);
}
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;
}
bool_t nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
{
static bool_t is_last_half = FALSE;
static float survey_radius;
nav_survey_active = TRUE;
/* entry scan */ // wait for start position and altitude be reached
if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0))
|| (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
} else {
if (!nav_survey_rectangle_active) {
nav_survey_rectangle_active = TRUE;
LINE_START_FUNCTION;
}
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));
/* Update the current segment from corners' coordinates*/
if (SurveyGoingNorth()) {
survey_to.y = nav_survey_north;
survey_from.y = nav_survey_south;
} else if (SurveyGoingSouth()) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else if (SurveyGoingEast()) {
survey_to.x = nav_survey_east;
survey_from.x = nav_survey_west;
} else if (SurveyGoingWest()) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
}
if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
/* if you like to use position croos instead of approaching uncoment this line
if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
(stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
(stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
(stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
*/
/* Continue ... */
ENU_BFP_OF_REAL(survey_to_i, survey_to);
if (!nav_approaching_from(&survey_to_i, NULL, 0)) {
ENU_BFP_OF_REAL(survey_from_i, survey_from);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(&survey_from_i, &survey_to_i);
} else {
if (survey_orientation == NS) {
/* North or South limit reached, prepare turn and next leg */
float x0 = survey_from.x; /* Current longitude */
if ((x0 + nav_survey_shift < nav_survey_west)
|| (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep
if (((x0 + (nav_survey_shift / 2)) < nav_survey_west)
|| ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep
if (is_last_half) {// was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
}else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = FALSE;
} else { // last sweep not half
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
}else{
survey_radius = nav_survey_shift ;
}
}
rectangle_survey_sweep_num ++;
} else { //room for half sweep after
survey_radius = nav_survey_shift / 2.;
is_last_half = TRUE;
}
} else {// room for full sweep after
survey_radius = nav_survey_shift;
}
x0 = x0 + survey_radius; /* Longitude of next leg */
survey_from.x = survey_to.x = x0;
/* Swap South and North extremities */
float tmp = survey_from.y;
survey_from.y = survey_to.y;
survey_to.y = tmp;
/** Do half a circle around WP 0 */
waypoints[0].enu_f.x = x0;
waypoints[0].enu_f.y = survey_from.y;
/* Computes the right direction */
if (SurveyGoingEast()) {
survey_radius = -survey_radius;
}
} else { /* (survey_orientation == WE) */
/* East or West limit reached, prepare turn and next leg */
/* There is a y0 declared in math.h (for ARM) !!! */
float my_y0 = survey_from.y; /* Current latitude */
if (my_y0 + nav_survey_shift < nav_survey_south
|| my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep
if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south)
|| ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep
if (is_last_half) {// was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
}else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = FALSE;
} else { // last sweep not half
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
}else{
survey_radius = nav_survey_shift ;
}
}
rectangle_survey_sweep_num ++;
} else { //room for half sweep after
survey_radius = nav_survey_shift / 2.;
is_last_half = TRUE;
}
} else {// room for full sweep after
survey_radius = nav_survey_shift;
}
my_y0 = my_y0 + survey_radius; /* latitude of next leg */
survey_from.y = survey_to.y = my_y0;
/* Swap West and East extremities */
float tmp = survey_from.x;
survey_from.x = survey_to.x;
survey_to.x = tmp;
/** Do half a circle around WP 0 */
waypoints[0].enu_f.x = survey_from.x;
waypoints[0].enu_f.y = my_y0;
/* Computes the right direction */
if (SurveyGoingNorth()) {
survey_radius = -survey_radius;
}
}
nav_in_segment = FALSE;
survey_uturn = TRUE;
LINE_STOP_FUNCTION;
#ifdef DIGITAL_CAM
float temp;
if (survey_orientation == NS) {
temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval;
} else{
temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval;
}
double inteiro;
double fract = modf (temp , &inteiro);
if (fract > .5) {
dc_send_command(DC_SHOOT); //if last shot is more than shot_distance/2 from the corner take a picture in the corner before go to the next sweep
}
#endif
}
} else { /* START turn */
static struct EnuCoor_f temp_f;
if (survey_orientation == WE) {
temp_f.x = waypoints[0].enu_f.x;
temp_f.y = waypoints[0].enu_f.y - survey_radius;
} else {
temp_f.y = waypoints[0].enu_f.y;
temp_f.x = waypoints[0].enu_f.x - survey_radius;
}
//detect when segment has done
/* if you like to use position croos instead of approaching uncoment this line
if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )||
(stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )||
(stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )||
(stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) {
*/
if (survey_orientation == WE) {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
} else {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
}
if (nav_approaching_from(&survey_to_i, NULL, 0)) {
survey_uturn = FALSE;
nav_in_circle = FALSE;
LINE_START_FUNCTION;
} else {
if (survey_orientation == WE) {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
} else {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
}
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(&survey_from_i, &survey_to_i);
}
} /* END turn */
} /* END entry scan */
return TRUE;
}// /* END survey_retangle */
bool_t SpiralNav(void)
{
TransCurrentX = estimator_x - WaypointX(Center);
TransCurrentY = estimator_y - WaypointY(Center);
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
float DistanceStartEstim;
float CircleAlpha;
switch(CSpiralStatus)
{
case Outside:
//flys until center of the helix is reached an start helix
nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center));
// center reached?
if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) {
// nadir image
#ifdef DIGITAL_CAM
dc_send_command(DC_SHOOT);
#endif
CSpiralStatus = StartCircle;
}
break;
case StartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to Circle
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
if(DistanceFromCenter >= SRad){
LastCircleX = estimator_x;
LastCircleY = estimator_y;
CSpiralStatus = Circle;
// Start helix
#ifdef DIGITAL_CAM
dc_Circle(360/Segmente);
#endif
}
break;
case Circle: {
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
// equation:
// alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x))
+ ((LastCircleY-estimator_y)*(LastCircleY-estimator_y)));
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
if (CircleAlpha >= Alphalimit) {
LastCircleX = estimator_x;
LastCircleY = estimator_y;
CSpiralStatus = IncSpiral;
}
break;
}
case IncSpiral:
// increasing circle radius as long as it is smaller than max helix radius
if(SRad + IRad < Spiralradius)
{
SRad = SRad + IRad;
#ifdef DIGITAL_CAM
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
TransCurrentZ = estimator_z - ZPoint;
dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI;
}
#endif
}
else {
SRad = Spiralradius;
#ifdef DIGITAL_CAM
// Stopps DC
dc_stop();
#endif
}
CSpiralStatus = Circle;
break;
default:
break;
}
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;
}
}