Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

set flight altitude independently from waypoints alt

Browse Source
This commit is contained in:
Gautier Hattenberger
2009-07-29 13:18:21 +00:00
parent 862855b645
commit 62b1df5a54
3 changed files with 29 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_navigation.c
+16 -1
View File
@@ -55,7 +55,10 @@ int32_t nav_heading, nav_course;
uint8_t vertical_mode;
uint32_t nav_throttle;
int32_t nav_climb, nav_altitude;
int32_t nav_climb, nav_altitude, nav_flight_altitude;
float flight_altitude;
static inline void nav_set_altitude( void );
#define CLOSE_TO_WAYPOINT (15 << 8)
#define ARRIVED_AT_WAYPOINT (10 << 8)
@@ -66,6 +69,8 @@ void booz2_nav_init(void) {
nav_stage = 0;
ground_alt = (int32_t)(GROUND_ALT * 100); // cm
nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
nav_flight_altitude = nav_altitude;
flight_altitude = SECURITY_HEIGHT;
INT32_VECT3_COPY( booz2_navigation_target, waypoints[WP_HOME]);
INT32_VECT3_COPY( booz2_navigation_carrot, waypoints[WP_HOME]);
@@ -102,6 +107,8 @@ void booz2_nav_run(void) {
VECT2_SDIV(path_to_carrot, dist_to_waypoint, path_to_carrot);
VECT2_SUM(booz2_navigation_carrot, path_to_carrot, booz_ins_enu_pos);
}
nav_set_altitude();
}
//#include "stdio.h"
@@ -157,6 +164,14 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
static int32_t previous_ground_alt;
static inline void nav_set_altitude( void ) {
static int32_t last_nav_alt = 0;
if (nav_altitude != last_nav_alt) {
nav_flight_altitude = nav_altitude;
last_nav_alt = nav_altitude;
}
}
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
booz_ins_ltp_initialised = FALSE;
sw/airborne/booz/booz2_navigation.h
+10 -3
View File
@@ -55,7 +55,8 @@ extern int32_t nav_heading, nav_course;
extern uint8_t vertical_mode;
extern uint32_t nav_throttle;
extern int32_t nav_climb, nav_altitude;
extern int32_t nav_climb, nav_altitude, nav_flight_altitude;
extern float flight_altitude;
#define VERTICAL_MODE_MANUAL 0
#define VERTICAL_MODE_CLIMB 1
#define VERTICAL_MODE_ALT 2
@@ -143,7 +144,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
/** Set the climb control to auto-throttle with the specified pitch
pre-command */
#define NavVerticalAutoThrottleMode(_pitch) { \
nav_pitch = _pitch; \
nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
}
/** Set the climb control to auto-pitch with the specified throttle
@@ -173,7 +174,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
#define NavAttitude(_roll) { \
horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
nav_roll = _roll; \
nav_roll = ANGLE_BFP_OF_REAL(_roll); \
}
#define nav_IncreaseShift(x) {}
@@ -183,4 +184,10 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
#define booz2_navigation_SetNavHeading(x) { \
nav_heading = ANGLE_BFP_OF_REAL(x); \
}
#define booz2_navigation_SetFlightAltitude(x) { \
flight_altitude = x; \
nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude); \
}
#endif /* BOOZ2_NAVIGATION_H */
sw/airborne/booz/guidance/booz2_guidance_v.c
+3 -5
View File
@@ -193,22 +193,20 @@ void booz2_guidance_v_run(bool_t in_flight) {
case BOOZ2_GUIDANCE_V_MODE_NAV:
{
if (vertical_mode == VERTICAL_MODE_ALT) {
booz2_guidance_v_z_sp = -nav_altitude;
booz2_guidance_v_z_sp = -nav_flight_altitude;
b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp);
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_CLIMB) {
booz2_guidance_v_zd_sp = -nav_climb;
b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
nav_altitude = -booz2_guidance_v_z_sp;
nav_flight_altitude = -booz2_guidance_v_z_sp;
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_MANUAL) {
booz2_guidance_v_delta_t = nav_throttle;
}
// saturate max authority with RC stick
booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
//booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t;
booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
break;
}
}