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

hysteresis on radio ok; ground detect use main event; rc not used in guidance if lost

Browse Source
This commit is contained in:
Gautier Hattenberger
2009-09-09 09:08:33 +00:00
parent 24b33c5df6
commit ddf825c9a2
9 changed files with 50 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_autopilot.c
+2
View File
@@ -39,6 +39,7 @@ uint32_t booz2_autopilot_in_flight_counter;
bool_t kill_throttle;
bool_t booz2_autopilot_detect_ground;
bool_t booz2_autopilot_detect_ground_once;
#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME 40
#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40
@@ -54,6 +55,7 @@ void booz2_autopilot_init(void) {
booz2_autopilot_in_flight_counter = 0;
booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2;
booz2_autopilot_detect_ground = FALSE;
booz2_autopilot_detect_ground_once = FALSE;
}
sw/airborne/booz/booz2_autopilot.h
+8 -5
View File
@@ -56,6 +56,7 @@ extern void booz2_autopilot_on_rc_frame(void);
extern void booz2_autopilot_set_mode(uint8_t new_autopilot_mode);
extern bool_t booz2_autopilot_detect_ground;
extern bool_t booz2_autopilot_detect_ground_once;
#ifndef BOOZ2_MODE_MANUAL
#define BOOZ2_MODE_MANUAL BOOZ2_AP_MODE_RATE_DIRECT
@@ -88,11 +89,13 @@ extern bool_t booz2_autopilot_detect_ground;
#define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
#define BoozDetectGroundEvent() { \
if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE) { \
if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT || \
booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) \
booz2_autopilot_detect_ground = TRUE; \
} \
if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || booz2_autopilot_detect_ground_once) { \
if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT || \
booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) { \
booz2_autopilot_detect_ground = TRUE; \
booz2_autopilot_detect_ground_once = FALSE; \
} \
} \
}
#endif /* BOOZ2_AUTOPILOT_H */
sw/airborne/booz/booz2_navigation.c
+9 -2
View File
@@ -137,7 +137,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
struct Int32Vect2 pos_diff;
VECT2_DIFF(pos_diff, booz_ins_enu_pos,waypoints[wp_center]);
// go back to half metric precision or values are too large
INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC-1);
//INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
// store last qdr
int32_t last_qdr = nav_circle_qdr;
// compute qdr
@@ -153,7 +153,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
int32_t abs_radius = abs(radius);
// carrot_angle
int32_t carrot_angle = (CARROT_DIST / abs_radius) << (INT32_TRIG_FRAC - INT32_POS_FRAC);
Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_2);
carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
int32_t s_carrot, c_carrot;
PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
@@ -182,6 +182,7 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
INT32_SQRT(leg_length,leg_length2);
int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / leg_length;
nav_leg_progress += Max((CARROT_DIST >> INT32_POS_FRAC), 0);
Bound(nav_leg_progress,0,leg_length);
struct Int32Vect2 progress_pos;
VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
VECT2_SDIV(progress_pos, progress_pos, leg_length);
@@ -287,5 +288,11 @@ void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
}
}
bool_t nav_detect_ground(void) {
if (!booz2_autopilot_detect_ground) return FALSE;
booz2_autopilot_detect_ground = FALSE;
return TRUE;
}
void nav_home(void) {}
sw/airborne/booz/booz2_navigation.h
+4 -3
View File
@@ -74,6 +74,7 @@ unit_t nav_reset_reference( void ) __attribute__ ((unused));
unit_t nav_reset_alt( void ) __attribute__ ((unused));
void nav_periodic_task(void);
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
bool_t nav_detect_ground(void);
void nav_home(void);
@@ -107,7 +108,7 @@ void nav_home(void);
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
#define NavSetWaypointHere(_wp) { FALSE; }
#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], booz_ins_enu_pos); FALSE; })
#define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
#define WaypointY(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].y)
@@ -189,8 +190,8 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
nav_roll = ANGLE_BFP_OF_REAL(_roll); \
}
#define NAV_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
#define NavDetectGround() ( booz_ins_enu_accel.z < -NAV_GROUND_DETECT || booz_ins_enu_accel.z > NAV_GROUND_DETECT )
#define NavStartDetectGround() ({ booz2_autopilot_detect_ground_once = TRUE; FALSE; })
#define NavDetectGround() nav_detect_ground()
#define nav_IncreaseShift(x) {}
sw/airborne/booz/booz_radio_control.c
+4 -1
View File
@@ -33,6 +33,7 @@ void radio_control_init(void) {
radio_control.values[i] = 0;
radio_control.status = RADIO_CONTROL_REALLY_LOST;
radio_control.time_since_last_frame = RADIO_CONTROL_REALLY_LOST_TIME;
radio_control.radio_ok_cpt = 0;
radio_control.frame_rate = 0;
radio_control.frame_cpt = 0;
radio_control_impl_init();
@@ -52,8 +53,10 @@ void radio_control_periodic(void) {
radio_control.status = RADIO_CONTROL_REALLY_LOST;
}
else {
if (radio_control.time_since_last_frame >= RADIO_CONTROL_LOST_TIME)
if (radio_control.time_since_last_frame >= RADIO_CONTROL_LOST_TIME) {
radio_control.status = RADIO_CONTROL_LOST;
radio_control.radio_ok_cpt = RADIO_CONTROL_OK_CPT;
}
radio_control.time_since_last_frame++;
}
sw/airborne/booz/booz_radio_control.h
+3
View File
@@ -40,10 +40,13 @@ extern void radio_control_impl_init(void);
/* timeouts - for now assumes 60Hz periodic */
#define RADIO_CONTROL_LOST_TIME 30
#define RADIO_CONTROL_REALLY_LOST_TIME 60
/* number of valid frames before going back to OK */
#define RADIO_CONTROL_OK_CPT 15
struct RadioControl {
uint8_t status;
uint8_t time_since_last_frame;
uint8_t radio_ok_cpt;
uint8_t frame_rate;
uint8_t frame_cpt;
pprz_t values[RADIO_CONTROL_NB_CHANNEL];
sw/airborne/booz/guidance/booz2_guidance_h.c
+9 -2
View File
@@ -138,8 +138,13 @@ void booz2_guidance_h_read_rc(bool_t in_flight) {
break;
case BOOZ2_GUIDANCE_H_MODE_NAV:
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
booz2_guidance_h_rc_sp.psi = 0;
if (radio_control.status == RADIO_CONTROL_OK) {
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
booz2_guidance_h_rc_sp.psi = 0;
}
else {
INT_EULERS_ZERO(booz2_guidance_h_rc_sp);
}
break;
}
@@ -164,6 +169,8 @@ void booz2_guidance_h_run(bool_t in_flight) {
case BOOZ2_GUIDANCE_H_MODE_NAV:
{
if (!in_flight) booz2_guidance_h_nav_enter();
if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
booz_stab_att_sp_euler.phi = nav_roll << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
sw/airborne/booz/guidance/booz2_guidance_v.c
+5 -2
View File
@@ -209,8 +209,11 @@ void booz2_guidance_v_run(bool_t in_flight) {
booz2_guidance_v_z_sp = -nav_flight_altitude; // For display only
booz2_guidance_v_delta_t = nav_throttle;
}
//booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
/* use rc limitation if available */
if (radio_control.status == RADIO_CONTROL_OK)
booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
else
booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
break;
}
}
sw/airborne/booz/radio_control/booz_radio_control_ppm.h
+6 -3
View File
@@ -51,9 +51,12 @@ extern volatile bool_t booz_radio_control_ppm_frame_available;
if (booz_radio_control_ppm_frame_available) { \
radio_control.frame_cpt++; \
radio_control.time_since_last_frame = 0; \
radio_control.status = RADIO_CONTROL_OK; \
NormalizePpm(); \
_received_frame_handler(); \
if (radio_control.radio_ok_cpt > 0) radio_control.radio_ok_cpt--; \
else { \
radio_control.status = RADIO_CONTROL_OK; \
NormalizePpm(); \
_received_frame_handler(); \
} \
booz_radio_control_ppm_frame_available = FALSE; \
} \
}