Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 06:54:49 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[fixedwing] refactor zamboni_survey

Browse Source
This commit is contained in:
Felix Ruess
2013-08-21 15:25:50 +02:00
parent e52daa73de
commit ac925bb0bb
3 changed files with 206 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_float.h
+5
View File
@@ -127,6 +127,11 @@ struct FloatRates {
n = sqrtf((v).x*(v).x + (v).y*(v).y); \ n = sqrtf((v).x*(v).x + (v).y*(v).y); \
} }
#define FLOAT_VECT2_NORMALIZE(_v) { \
const float n = sqrtf((_v).x*(_v).x + (_v).y*(_v).y); \
FLOAT_VECT2_SMUL(_v, _v, 1./n); \
}
/* /*
* Dimension 3 Vectors * Dimension 3 Vectors
sw/airborne/subsystems/navigation/zamboni_survey.c
+134 -162
View File
@@ -1,3 +1,30 @@
/*
* Copyright (C) 2013 Jorn Anke, Felix Ruess
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/zamboni_survey.c
*
* Zamboni pattern survey for fixedwings.
*/
#include "zamboni_survey.h" #include "zamboni_survey.h"
#include "subsystems/nav.h" #include "subsystems/nav.h"
@@ -8,230 +35,175 @@
#include "modules/digital_cam/dc.h" #include "modules/digital_cam/dc.h"
#endif #endif
int counter;
/** struct ZamboniSurvey zs;
variables used to store values from the flight plan
**/
float x_wp_center, y_wp_center;
float x_wp_dir, y_wp_dir;
float z_sweep_length;
float z_sweep_spacing;
int z_sweep_lines;
float z_shot_dist;
float z_altitude;
/** /**
static variables, used for initial calculations * initializes the variables needed for the survey to start.
**/ *
// properties for the filightpattern * @param center_wp the waypoint defining the center of the survey-rectangle
float flight_angle, zamboni_return_angle; * @param dir_wp the waypoint defining the orientation of the survey-rectangle
float dx_sweep_width, dy_sweep_width; * @param sweep_length the length of the survey-rectangle
float dx_flightline, dy_flightline; * @param sweep_spacing distance between the sweeps
float dx_flight_vec, dy_flight_vec; * @param sweep_lines number of sweep_lines to fly
float turnradius1, turnradius2; * @param altitude the altitude that must be reached before the flyover starts
int laps; */
// points for navigation
float x_seg_start, y_seg_start;
float x_seg_end, y_seg_end;
float x_turn_center1, y_turn_center1;
float x_turn_center2, y_turn_center2;
float x_ret_start, y_ret_start;
float x_ret_end, y_ret_end;
// variables used for initial calculations
float dx_flight_wpts, dy_flight_wpts;
float len;
// constant for storing value for pre-leave-angle, (leave turncircles a small angle before the 180deg turns are compleated to get a smoother transition to flight-lines)
int pre_leave_angle=2;
z_survey_stage z_stage;
/*
z_stage starts at ENTRY and than circles trought the other
states until to rectangle is completely covered
ENTRY : getting in the right position and height for the first flyover
SEG : fly from seg_start to seg_end and take pictures,
then calculate navigation points of next flyover
TURN1 : do a 180° turn around seg_center1
RET : fly from ret_start to ret_end
TURN2 : do a 180° turn around seg_center2
*/
/**
initializes the variables needed for the survey to start
wp_center : the waypoint defining the center of the survey-rectangle
wp_dir : the waypoint defining the orientation of the survey-rectangle
sweep_length : the length of the survey-rectangle
sweep_spacing : distance between the sweeps
sweep_lines : number of sweep_lines to fly
altitude : the altitude that must be reached before the flyover starts
**/
bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude) bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
{ {
counter = 0; zs.current_laps = 0;
zs.pre_leave_angle = 2;
// copy variables from the flight plan // copy variables from the flight plan
x_wp_center = waypoints[center_wp].x; VECT2_COPY(zs.wp_center, waypoints[center_wp]);
y_wp_center = waypoints[center_wp].y; VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
x_wp_dir = waypoints[dir_wp].x; zs.altitude = altitude;
y_wp_dir = waypoints[dir_wp].y;
z_sweep_length = sweep_length;
z_sweep_spacing = sweep_spacing;
z_sweep_lines = sweep_lines;
//z_shot_dist = shot_dist;
z_altitude = altitude;
// if turning right leave circle before angle is reached, if turning left - leave after // if turning right leave circle before angle is reached, if turning left - leave after
if (z_sweep_spacing>0) pre_leave_angle -= pre_leave_angle; 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 // calculate the flight_angle
dx_flight_wpts = x_wp_dir - x_wp_center; zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
dy_flight_wpts = y_wp_dir - y_wp_center; zs.return_angle = zs.flight_angle + 180;
if (dy_flight_wpts == 0) dy_flight_wpts = 0.01; // to avoid dividing by zero if (zs.return_angle > 359) {
flight_angle = 180*atan2(dx_flight_wpts, dy_flight_wpts)/M_PI; zs.return_angle -= 360;
zamboni_return_angle = flight_angle + 180; }
if (zamboni_return_angle > 359) zamboni_return_angle -= 360;
// calculate the flightline vector from start to end of one flightline, (delta x and delta y for one flightline) // 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) // (used later to move the flight pattern one flightline up for each round)
len = sqrtf(dx_flight_wpts * dx_flight_wpts + dy_flight_wpts * dy_flight_wpts); zs.sweep_width.x = -flight_vec.y * sweep_spacing;
dx_flight_vec = dx_flight_wpts/len; zs.sweep_width.y = +flight_vec.x * sweep_spacing;
dy_flight_vec = dy_flight_wpts/len;
dx_flightline = dx_flight_vec * z_sweep_length;
dy_flightline = dy_flight_vec * z_sweep_length;
// 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)
dx_sweep_width = -(dy_flight_wpts/len) * z_sweep_spacing;
dy_sweep_width = +(dx_flight_wpts/len) * z_sweep_spacing;
// calculate number of laps to fly and turning radius for each end // calculate number of laps to fly and turning radius for each end
laps = (z_sweep_lines+1)/2; zs.total_laps = (sweep_lines+1)/2;
turnradius1 = (laps-1) * z_sweep_spacing * 0.5; zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
turnradius2 = laps * z_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 //CALCULATE THE NAVIGATION POINTS
//start and end of flight-line in flight-direction //start and end of flight-line in flight-direction
x_seg_start = x_wp_center - dx_flightline * 0.5; VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
y_seg_start = y_wp_center - dy_flightline * 0.5; VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
x_seg_end = x_wp_center + dx_flightline * 0.5;
y_seg_end = y_wp_center + dy_flightline * 0.5;
struct FloatVect2 sweep_span;
VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
//start and end of flight-line in return-direction //start and end of flight-line in return-direction
x_ret_start = x_seg_end - dx_sweep_width * (laps-1); VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
y_ret_start = y_seg_end - dy_sweep_width * (laps-1); VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
x_ret_end = x_seg_start - dx_sweep_width * (laps-1);
y_ret_end = y_seg_start - dy_sweep_width * (laps-1);
//turn-centers at both ends //turn-centers at both ends
x_turn_center1 = (x_seg_end + x_ret_start)/2; zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
y_turn_center1 = (y_seg_end + y_ret_start)/2; zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
x_turn_center2 = (x_seg_start + x_ret_end + dx_sweep_width) / 2; zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
y_turn_center2 = (y_seg_start + y_ret_end + dy_sweep_width) / 2; zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;
//fast climbing to desired altitude //fast climbing to desired altitude
NavVerticalAutoThrottleMode(100.0); NavVerticalAutoThrottleMode(100.0);
NavVerticalAltitudeMode(z_altitude, 0.0); NavVerticalAltitudeMode(zs.altitude, 0.0);
z_stage = Z_ENTRY; zs.stage = Z_ENTRY;
return FALSE; return FALSE;
} }
/** /**
main navigation routine. This is called periodically evaluates the current * main navigation routine.
Position and stage and navigates accordingly. * This is called periodically evaluates the current
Returns True until the survey is finished * Position and stage and navigates accordingly.
**/ *
* @returns TRUE until the survey is finished
*/
bool_t zamboni_survey(void) bool_t zamboni_survey(void)
{ {
// retain altitude // retain altitude
NavVerticalAutoThrottleMode(0.0); NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(z_altitude, 0.0); NavVerticalAltitudeMode(zs.altitude, 0.0);
//go from center of field to end of field - (before starting the syrvey) //go from center of field to end of field - (before starting the syrvey)
if (z_stage == Z_ENTRY) { if (zs.stage == Z_ENTRY) {
nav_route_xy(x_wp_center, y_wp_center, x_seg_end, y_seg_end); nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y);
if (nav_approaching_xy(x_seg_end, y_seg_end, x_wp_center, y_wp_center, CARROT)) { if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) {
z_stage = Z_TURN1; zs.stage = Z_TURN1;
NavVerticalAutoThrottleMode(0.0); NavVerticalAutoThrottleMode(0.0);
nav_init_stage(); nav_init_stage();
} }
} }
//Turn from stage to return //Turn from stage to return
else if (z_stage == Z_TURN1) { else if (zs.stage == Z_TURN1) {
nav_circle_XY(x_turn_center1, y_turn_center1, turnradius1); nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1);
if (NavCourseCloseTo(zamboni_return_angle+pre_leave_angle)){ if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){
// && nav_approaching_xy(x_seg_end, y_seg_end, x_seg_start, y_seg_start, CARROT // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
//calculate SEG-points for the next flyover //calculate SEG-points for the next flyover
x_seg_start = x_seg_start + dx_sweep_width; VECT2_ADD(zs.seg_start, zs.sweep_width);
y_seg_start = y_seg_start + dy_sweep_width; VECT2_ADD(zs.seg_end, zs.sweep_width);
x_seg_end = x_seg_end + dx_sweep_width;
y_seg_end = y_seg_end + dy_sweep_width;
z_stage = Z_RET; zs.stage = Z_RET;
nav_init_stage(); nav_init_stage();
#ifdef DIGITAL_CAM #ifdef DIGITAL_CAM
//dc_survey(z_shot_dist, x_ret_start - dx_flight_vec * z_shot_dist, y_ret_start - dy_flight_vec * z_shot_dist); LINE_START_FUNCTION;
LINE_START_FUNCTION; #endif
#endif
} }
} }
//fly the segment until seg_end is reached //fly the segment until seg_end is reached
else if (z_stage == Z_RET) { else if (zs.stage == Z_RET) {
nav_route_xy(x_ret_start, y_ret_start, x_ret_end, y_ret_end); nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y);
if (nav_approaching_xy(x_ret_end, y_ret_end, x_ret_start, y_ret_start, 0)) { if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) {
counter = counter + 1; zs.current_laps = zs.current_laps + 1;
#ifdef DIGITAL_CAM #ifdef DIGITAL_CAM
//dc_stop(); //dc_stop();
LINE_STOP_FUNCTION; LINE_STOP_FUNCTION;
#endif #endif
z_stage = Z_TURN2; zs.stage = Z_TURN2;
} }
} }
//turn from stage to return //turn from stage to return
else if (z_stage == Z_TURN2) { else if (zs.stage == Z_TURN2) {
nav_circle_XY(x_turn_center2, y_turn_center2, turnradius2); nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2);
if (NavCourseCloseTo(flight_angle+pre_leave_angle)) { if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) {
//counter = counter + 1; //zs.current_laps = zs.current_laps + 1;
z_stage = Z_SEG; zs.stage = Z_SEG;
nav_init_stage(); nav_init_stage();
#ifdef DIGITAL_CAM #ifdef DIGITAL_CAM
//dc_survey(z_shot_dist, x_seg_start + dx_flight_vec * z_shot_dist, y_seg_start + dy_flight_vec * z_shot_dist); LINE_START_FUNCTION;
LINE_START_FUNCTION; #endif
#endif
} }
//return //return
} else if (z_stage == Z_SEG) { } else if (zs.stage == Z_SEG) {
nav_route_xy(x_seg_start, y_seg_start, x_seg_end, y_seg_end); nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y);
if (nav_approaching_xy(x_seg_end, y_seg_end, x_seg_start, y_seg_start, 0)) { if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) {
// calculate the rest of the points for the next fly-over // calculate the rest of the points for the next fly-over
x_ret_start = x_ret_start + dx_sweep_width; VECT2_ADD(zs.ret_start, zs.sweep_width);
y_ret_start = y_ret_start + dy_sweep_width; VECT2_ADD(zs.ret_end, zs.sweep_width);
x_ret_end = x_ret_end + dx_sweep_width; zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2;
y_ret_end = y_ret_end + dy_sweep_width; zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2;
x_turn_center1 = (x_seg_end + x_ret_start)/2; zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2;
y_turn_center1 = (y_seg_end + y_ret_start)/2; zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2;
x_turn_center2 = (x_seg_start + x_ret_end + dx_sweep_width) / 2;
y_turn_center2 = (y_seg_start + y_ret_end + dy_sweep_width) / 2;
z_stage = Z_TURN1; zs.stage = Z_TURN1;
nav_init_stage(); nav_init_stage();
#ifdef DIGITAL_CAM #ifdef DIGITAL_CAM
//dc_stop(); //dc_stop();
LINE_STOP_FUNCTION; LINE_STOP_FUNCTION;
#endif #endif
} }
} }
if (counter >= laps) { if (zs.current_laps >= zs.total_laps) {
#ifdef DIGITAL_CAM #ifdef DIGITAL_CAM
LINE_STOP_FUNCTION; LINE_STOP_FUNCTION;
#endif #endif
return FALSE; return FALSE;
} }
else { else {
sw/airborne/subsystems/navigation/zamboni_survey.h
+67 -4
View File
@@ -1,13 +1,76 @@
#ifndef ZAMBONI_H /*
#define ZAMBONI_H * Copyright (C) 2013 Jorn Anke, Felix Ruess
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/zamboni_survey.h
*
* Zamboni pattern survey for fixedwings.
*/
#ifndef ZAMBONI_SURVEY_H
#define ZAMBONI_SURVEY_H
#include "std.h" #include "std.h"
#include "math/pprz_algebra_float.h"
//typedef struct {float x; float y;} point2d;
typedef enum {Z_ERR, Z_ENTRY, Z_SEG, Z_TURN1, Z_RET, Z_TURN2} z_survey_stage; typedef enum {Z_ERR, Z_ENTRY, Z_SEG, Z_TURN1, Z_RET, Z_TURN2} z_survey_stage;
struct ZamboniSurvey {
/* variables used to store values from the flight plan */
struct FloatVect2 wp_center;
struct FloatVect2 wp_dir;
struct FloatVect2 sweep_width;
float altitude;
/** in degrees. Leave turncircles a small angle before the 180deg turns are completed
* to get a smoother transition to flight-lines
*/
int pre_leave_angle;
float flight_angle; ///< in degrees
float return_angle; ///< in degrees
int current_laps;
int total_laps;
float turnradius1;
float turnradius2;
struct FloatVect2 turn_center1;
struct FloatVect2 turn_center2;
struct FloatVect2 seg_start;
struct FloatVect2 seg_end;
struct FloatVect2 ret_start;
struct FloatVect2 ret_end;
/**
* z_stage starts at ENTRY and than circles trought the other
* states until to rectangle is completely covered
* ENTRY : getting in the right position and height for the first flyover
* SEG : fly from seg_start to seg_end and take pictures,
* then calculate navigation points of next flyover
* TURN1 : do a 180° turn around seg_center1
* RET : fly from ret_start to ret_end
* TURN2 : do a 180° turn around seg_center2
*/
z_survey_stage stage;
};
extern bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude); extern bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude);
extern bool_t zamboni_survey(void); extern bool_t zamboni_survey(void);
#endif #endif //ZAMBONI_SURVEY_H