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

Add GPS Failsafe mode.

Browse Source
This commit is contained in:
Louis Dugrain
2005-08-27 23:51:00 +00:00
parent 3f34f371d7
commit d5e847d283
4 changed files with 81 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/autopilot/autopilot.h
+6 -5
View File
@@ -31,11 +31,12 @@
#define TRESHOLD2 200 * CLOCK
#define PPRZ_MODE_MANUAL 0
#define PPRZ_MODE_AUTO1 1
#define PPRZ_MODE_AUTO2 2
#define PPRZ_MODE_HOME 3
#define PPRZ_MODE_NB 4
#define PPRZ_MODE_MANUAL 0
#define PPRZ_MODE_AUTO1 1
#define PPRZ_MODE_AUTO2 2
#define PPRZ_MODE_HOME 3
#define PPRZ_MODE_GPS_OUT_OF_ORDER 4
#define PPRZ_MODE_NB 5
#define PPRZ_MODE_OF_PULSE(pprz, mega8_status) \
(pprz > TRESHOLD2 ? PPRZ_MODE_AUTO2 : \
sw/airborne/autopilot/main.c
+45 -24
View File
@@ -82,6 +82,7 @@ float slider_1_val, slider_2_val;
bool_t launch = FALSE;
float energy; /** Fuel consumption */
static uint16_t last_gps_msg_t; /** cputime of the last gps message */
#define Min(x, y) (x < y ? x : y)
@@ -106,7 +107,9 @@ float energy; /** Fuel consumption */
*/
inline uint8_t pprz_mode_update( void ) {
/** We remain in home mode until explicit reset from the RC */
if (pprz_mode != PPRZ_MODE_HOME || CheckEvent(rc_event_1)) {
if ((pprz_mode != PPRZ_MODE_HOME &&
pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER)
|| CheckEvent(rc_event_1)) {
ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(from_fbw.channels[RADIO_MODE], from_fbw.status));
} else
return FALSE;
@@ -183,13 +186,6 @@ inline void copy_from_to_fbw ( void ) {
to_fbw.status = 0;
}
#ifdef EST_TEST
float est_pos_x;
float est_pos_y;
float est_fcourse;
uint8_t ticks_last_est; // 20Hz
#endif /* EST_TEST */
/*
@@ -243,6 +239,14 @@ uint8_t ac_ident = AC_ID;
#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&climb_sum_err, &climb_pgain, &course_pgain)
#define PERIODIC_SEND_CIRCLE() if (in_circle) { DOWNLINK_SEND_CIRCLE(&circle_x, &circle_y, &circle_radius); }
#define PERIODIC_SEND_SEGMENT() if (in_segment) { DOWNLINK_SEND_SEGMENT(&segment_x_1, &segment_y_1, &segment_x_2, &segment_y_2); }
#define PERIODIC_SEND_CALIB_START() if (!estimator_flight_time && calib_status == WAITING_CALIB_CONTRAST) { DOWNLINK_SEND_CALIB_START(); }
#define PERIODIC_SEND_CALIB_CONTRAST() if (!estimator_flight_time && calib_status == CALIB_DONE) { DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast); }
/** Status of the calibration. Can be one of the \a calibration \a states */
static uint8_t calib_status = NO_CALIB;
@@ -342,7 +346,7 @@ inline void radio_control_task( void ) {
#else
/** In Auto1 mode, roll is bounded between [-0.6;0.6] */
desired_roll = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_ROLL], 0., -0.6);
#endif
#endif /* AUTO1_MAX_ROLL */
#ifdef AUTO1_MAX_PITCH
/** In Auto1 mode, pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
@@ -350,7 +354,7 @@ inline void radio_control_task( void ) {
#else
/** In Auto1 mode, pitch is bounded between [-0.5;0.5] */
desired_pitch = FLOAT_OF_PPRZ(from_fbw.channels[RADIO_PITCH], 0., 0.5);
#endif
#endif /* AUTO1_MAX_PITCH */
}
if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
desired_gaz = from_fbw.channels[RADIO_THROTTLE];
@@ -380,11 +384,38 @@ inline void radio_control_task( void ) {
* \brief Compute desired_course
*/
void navigation_task( void ) {
static uint8_t last_pprz_mode;
static bool_t has_lost_gps = FALSE;
/** This section is used for the failsafe of GPS */
#ifdef FAILSAFE_DELAY_WITHOUT_GPS
/** Test if we lost the GPS */
if (!GPS_FIX_VALID(gps_mode) ||
(cputime - last_gps_msg_t > FAILSAFE_DELAY_WITHOUT_GPS)) {
/** If aircraft is launch and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe). */
if (launch && (pprz_mode == PPRZ_MODE_AUTO2 ||
pprz_mode == PPRZ_MODE_HOME)) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
PERIODIC_SEND_PPRZ_MODE();
has_lost_gps = TRUE;
}
}
/** If aircraft was in failsafe mode, come back in previous mode */
else if (has_lost_gps) {
pprz_mode = last_pprz_mode;
has_lost_gps = FALSE;
PERIODIC_SEND_PPRZ_MODE();
}
#endif /* FAILSAFE_DELAY_WITHOUT_GPS */
/** Default to keep compatibility with previous behaviour */
lateral_mode = LATERAL_MODE_COURSE;
if (pprz_mode == PPRZ_MODE_HOME)
nav_home();
else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
nav_wihtout_gps();
else
nav_update();
@@ -400,7 +431,8 @@ void navigation_task( void ) {
DOWNLINK_SEND_CAM(&phi, &theta, &x, &y);
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
if (lateral_mode >= LATERAL_MODE_COURSE)
course_pid_run(); /* aka compute nav_desired_roll */
desired_roll = nav_desired_roll;
@@ -470,18 +502,6 @@ inline void periodic_task( void ) {
static uint8_t t = 0;
if (vsupply < LOW_BATTERY) t++; else t = 0;
low_battery |= (t >= LOW_BATTERY_DELAY);
if (in_circle) {
DOWNLINK_SEND_CIRCLE(&circle_x, &circle_y, &circle_radius); }
if (in_segment) {
DOWNLINK_SEND_SEGMENT(&segment_x_1, &segment_y_1, &segment_x_2, &segment_y_2); }
if (!estimator_flight_time) {
if (calib_status == WAITING_CALIB_CONTRAST) {
DOWNLINK_SEND_CALIB_START();
} else if (calib_status == CALIB_DONE) {
DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast);
}
}
}
switch(_4Hz) {
case 0:
@@ -543,6 +563,7 @@ void use_gps_pos( void ) {
DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_itow, &gps_utm_zone);
estimator_update_state_gps();
SEND_RAD_OF_IR();
if (GPS_FIX_VALID(gps_mode)) last_gps_msg_t = cputime;
static uint8_t i;
if (i == gps_nb_channels) i = 0;
sw/airborne/autopilot/nav.c
+29
View File
@@ -287,7 +287,11 @@ static inline void compute_dist2_to_home(void) {
*/
void nav_home(void) {
/** FIXME: radius should be defined elsewhere */
#ifdef FAILSAFE_HOME_RADIUS
Circle(WP_HOME, FAILSAFE_HOME_RADIUS);
#else
Circle(WP_HOME, 50);
#endif
/** Nominal speed */
nav_pitch = 0.;
vertical_mode = VERTICAL_MODE_AUTO_ALT;
@@ -313,3 +317,28 @@ void nav_init(void) {
nav_block = 0;
nav_stage = 0;
}
/** void nav_wihtout_gps(void)
* \brief
*/
/** Don't navigate anymore. It's impossible without GPS.
* Just set attitude and gaz to failsafe values
* to prevent the plane from crashing.
*/
void nav_wihtout_gps(void) {
#ifdef SECTION_FAILSAFE
lateral_mode = LATERAL_MODE_ROLL;
nav_desired_roll = FAILSAFE_DEFAULT_ROLL;
auto_pitch = FALSE;
nav_pitch = FAILSAFE_DEFAULT_PITCH;
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
nav_desired_gaz = TRIM_UPPRZ(FAILSAFE_DEFAULT_GAZ*MAX_PPRZ);
#else
lateral_mode = LATERAL_MODE_ROLL;
nav_desired_roll = 0;
auto_pitch = FALSE;
nav_pitch = 0;
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
nav_desired_gaz = TRIM_UPPRZ(CLIMB_LEVEL_GAZ*MAX_PPRZ);
#endif
}
sw/airborne/autopilot/nav.h
+1
View File
@@ -76,5 +76,6 @@ extern uint8_t horizontal_mode;
void nav_update(void);
void nav_home(void);
void nav_init(void);
void nav_wihtout_gps(void);
#endif /* NAV_H */