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

*** empty log message ***

Browse Source
This commit is contained in:
Pascal Brisset
2007-03-07 14:37:05 +00:00
parent a13c5bcc1a
commit 080d1bfc36
13 changed files with 137 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/funjet1.xml
+4 -4
View File
@@ -145,15 +145,15 @@
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="ROLL_PGAIN" value="13000."/>
<define name="ROLL_PGAIN" value="10000."/>
<define name="AILERON_OF_THROTTLE" value="0.0"/>
<define name="PITCH_PGAIN" value="-14000."/>
<define name="PITCH_DGAIN" value="1.5"/>
<define name="ELEVATOR_OF_ROLL" value="1250"/>
<define name="ELEVATOR_OF_ROLL" value="2500"/>
<!-- roll rate loop -->
<define name="ROLL_RATE_MODE_DEFAULT" value="1"/>
<define name="ROLL_RATE_SETPOINT_PGAIN" value="-14." unit="rad/s/rad"/>
<define name="ROLL_RATE_SETPOINT_PGAIN" value="-10." unit="rad/s/rad"/>
<define name="ROLL_RATE_MAX_SETPOINT" value="10"/>
<define name="LO_THROTTLE_ROLL_RATE_PGAIN" value="1000."/>
<define name="HI_THROTTLE_ROLL_RATE_PGAIN" value="1000."/>
@@ -174,7 +174,7 @@
<define name="CLIMB_THROTTLE" value="0.8"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.15"/><!-- Pitch for Aggressive Decent -->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
conf/airframes/microjet6.xml
+3 -1
View File
@@ -98,7 +98,7 @@
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="14." unit="m/s"/>
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
@@ -168,6 +168,8 @@
<section name="NAV">
<define name="NAV_PITCH" value="0."/>
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
<define name="NAV_GROUND_SPEED_PGAIN" value="-0.05"/>
<define name="NAV_FOLLOW_PGAIN" value="-0.05"/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
conf/flight_plans/IS.xml
+5
View File
@@ -46,6 +46,11 @@
<circle radius="nav_radius" wp="1"/>
</block>
<block name="ground speed">
<!-- <exception cond="nav_ground_speed_loop()" deroute="ground speed"/> -->
<circle radius="nav_radius" wp="1"/>
</block>
<block name="eight 1" strip_button="8">
<eight radius="nav_radius" center="1" turn_around="2"/>
</block>
conf/flight_plans/versatile.xml
+5 -1
View File
@@ -33,7 +33,7 @@
</block>
<block name="circle 1">
<circle radius="75" wp="1"/>
<circle radius="nav_radius" wp="1"/>
</block>
<block name="eight 1">
@@ -133,5 +133,9 @@
</while>
</block>
<block name="heading">
<heading until="FALSE" course="30" alt="ground_alt+50"/>
</block>
</blocks>
</flight_plan>
conf/settings/tuning.xml
+14
View File
@@ -40,6 +40,19 @@
<dl_setting MAX="1" MIN="0" STEP="1" VAR="ir_360" shortname="ir_360"/>
</dl_settings>
<dl_settings name="rate">
<dl_setting MAX="1" MIN="0" STEP="0.1" VAR="h_ctl_roll_rate_mode" shortname="mode"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_hi_throttle_roll_rate_pgain" shortname="hi th pgain"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_lo_throttle_roll_rate_pgain" shortname="lo th pgain"/>
<dl_setting MAX="2" MIN="-2" STEP="0.1" VAR="h_ctl_roll_rate_dgain" shortname="dgain"/>
<dl_setting MAX="0" MIN="-20" STEP="1" VAR="h_ctl_roll_rate_setpoint_pgain" shortname="roll_pgain"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="h_ctl_auto1_rate" shortname="AUTO1 rate"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain"/>
<dl_setting MAX="000" MIN="-25000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain"/>
@@ -76,6 +89,7 @@
<dl_setting MAX="-0.1" MIN="-2" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain"/>
<dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim" shortname="glide pitch trim"/>
<dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_radius"/>
</dl_settings>
</dl_settings>
</dl_settings>
sw/airborne/fw_v_ctl.c
+2
View File
@@ -216,6 +216,8 @@ inline static void v_ctl_climb_auto_pitch_loop(void) {
#ifndef V_CTL_THROTTLE_SLEW
#define V_CTL_THROTTLE_SLEW 1.
#endif
/** \brief Computes slewed throttle from throttle setpoint
*/
void v_ctl_throttle_slew( void ) {
pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;
BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
sw/airborne/main_ap.c
-2
View File
@@ -319,8 +319,6 @@ static void navigation_task( void ) {
}
#endif /* GPS && 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)
sw/airborne/nav.c
+88 -45
View File
@@ -79,6 +79,8 @@ float flight_altitude;
float nav_glide_pitch_trim;
float nav_ground_speed_setpoint, nav_ground_speed_pgain;
void nav_init_stage( void ) {
last_x = estimator_x; last_y = estimator_y;
stage_time = 0;
@@ -206,13 +208,9 @@ void nav_circle_XY(float x, float y, float radius) {
#define NavBlockTime() (block_time)
#define LessThan(_x, _y) ((_x) < (_y))
#define NavFollow(_ac_id, _distance, _height) { \
struct ac_info_ * ac = get_ac_info(_ac_id); \
NavVerticalAutoThrottleMode(0.); \
NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.); \
float alpha = M_PI/2 - ac->course; \
fly_to_xy(ac->east - _distance*cos(alpha), ac->north - _distance*sin(alpha)); \
}
#define NavFollow(_ac_id, _distance, _height) \
nav_follow(_ac_id, _distance, _height);
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
@@ -317,12 +315,31 @@ static unit_t unit __attribute__ ((unused));
static unit_t nav_reset_reference( void ) __attribute__ ((unused));
static unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused));
static inline void nav_follow(uint8_t _ac_id, float _distance, float _height);
#include "flight_plan.h"
float ground_alt;
static float previous_ground_alt;
static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
struct ac_info_ * ac = get_ac_info(_ac_id);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.);
float alpha = M_PI/2 - ac->course;
float ca = cos(alpha), sa = sin(alpha);
float x = ac->east - _distance*ca;
float y = ac->north - _distance*sa;
fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
float s = (estimator_x-x)*ca+(estimator_y-y)*sa;
nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s;
nav_ground_speed_loop();
#endif
}
/** Reset the geographic reference to the current GPS fix */
static unit_t nav_reset_reference( void ) {
@@ -385,16 +402,20 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
}
/** static inline void fly_to_xy(float x, float y)
/**
* \brief Computes \a desired_x, \a desired_y and \a desired_course.
*/
static inline void fly_to_xy(float x, float y) {
desired_x = x;
desired_y = y;
h_ctl_course_setpoint = M_PI/2.-atan2(y - estimator_y, x - estimator_x);
lateral_mode = LATERAL_MODE_COURSE;
}
/**
* \brief Computes the carrot position along the desired segment.
*/
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
float leg_x = wp_x - last_wp_x;
float leg_y = wp_y - last_wp_y;
@@ -418,8 +439,8 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
}
/** \brief Compute square distance to waypoint Home and
* verify uav is not too far away from Home.
/** \brief Computes square distance to the HOME waypoint potentially sets
* \a too_far_from_home
*/
static inline void compute_dist2_to_home(void) {
float ph_x = waypoints[WP_HOME].x - estimator_x;
@@ -436,8 +457,7 @@ static inline void compute_dist2_to_home(void) {
#define FAILSAFE_HOME_RADIUS 50
#endif
/** \brief Occurs when it switchs in Home mode.
*/
/** \brief Home mode navigation (circle around HOME) */
void nav_home(void) {
NavCircleWaypoint(WP_HOME, FAILSAFE_HOME_RADIUS);
/** Nominal speed */
@@ -448,13 +468,17 @@ void nav_home(void) {
dist2_to_wp = dist2_to_home;
}
/** void nav_update(void)
* \brief Update navigation
/**
* \brief Navigation main: call to the code generated from the XML flight
* plan
*/
void nav_update(void) {
compute_dist2_to_home();
auto_nav();
auto_nav(); /* From flight_plan.h */
h_ctl_course_pre_bank = nav_in_circle ? circle_bank : 0;
#ifdef AGR_CLIMB
if ( v_ctl_mode == V_CTL_MODE_AUTO_CLIMB)
v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
@@ -469,40 +493,55 @@ void nav_update(void) {
}
/** void nav_init(void)
* \brief Initialisation of navigation
/**
* \brief Navigation Initialisation
*/
void nav_init(void) {
nav_block = 0;
nav_stage = 0;
ground_alt = GROUND_ALT;
nav_glide_pitch_trim = NAV_GLIDE_PITCH_TRIM;
nav_radius = DEFAULT_CIRCLE_RADIUS;
#ifdef NAV_GROUND_SPEED_PGAIN
nav_ground_speed_pgain = NAV_GROUND_SPEED_PGAIN;
nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
#endif
}
/** void nav_wihtout_gps(void)
* \brief
*/
/** Don't navigate anymore. It's impossible without GPS.
* Just set attitude and throttle to failsafe values
* to prevent the plane from crashing.
/**
* \brief Failsafe navigation without position estimation
*
* Just set attitude and throttle to FAILSAFE values
* to prevent the plane from crashing.
*/
void nav_without_gps(void) {
#ifdef SECTION_FAILSAFE
lateral_mode = LATERAL_MODE_ROLL;
v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
#ifdef SECTION_FAILSAFE
h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL;
nav_pitch = FAILSAFE_DEFAULT_PITCH;
v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE)*MAX_PPRZ);
#else
lateral_mode = LATERAL_MODE_ROLL;
h_ctl_roll_setpoint = 0;
nav_pitch = 0;
v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE;
nav_throttle_setpoint = TRIM_UPPRZ((CRUISE_THROTTLE)*MAX_PPRZ);
#endif
}
#ifdef NAV_GROUND_SPEED_PGAIN
/** \brief Computes cruise throttle from ground speed setpoint
*/
int nav_ground_speed_loop( void ) {
float err = estimator_hspeed_mod - nav_ground_speed_setpoint;
v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err;
Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
return FALSE;
}
#endif
/**************** 8 Navigation **********************************************/
@@ -542,24 +581,28 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
}
/* The other center */
struct point c2 = { waypoints[target].x - d*u_x,
waypoints[target].y - d*u_y,
alt };
struct point c2 = {
waypoints[target].x - d*u_x,
waypoints[target].y - d*u_y,
alt };
struct point c1_in = { waypoints[c1].x + radius * -u_y,
waypoints[c1].y + radius * u_x,
alt };
struct point c1_out = { waypoints[c1].x - radius * -u_y,
waypoints[c1].y - radius * u_x,
alt };
struct point c1_in = {
waypoints[c1].x + radius * -u_y,
waypoints[c1].y + radius * u_x,
alt };
struct point c1_out = {
waypoints[c1].x - radius * -u_y,
waypoints[c1].y - radius * u_x,
alt };
struct point c2_in = { c2.x + radius * -u_y,
c2.y + radius * u_x,
alt };
struct point c2_out = { c2.x - radius * -u_y,
c2.y - radius * u_x,
alt };
struct point c2_in = {
c2.x + radius * -u_y,
c2.y + radius * u_x,
alt };
struct point c2_out = {
c2.x - radius * -u_y,
c2.y - radius * u_x,
alt };
float qdr_out = M_PI - atan2(u_y, u_x);
@@ -586,8 +629,8 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
eight_status = R21;
InitStage();
}
return;
return;
case R21:
nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
sw/airborne/nav.h
+11
View File
@@ -112,6 +112,8 @@ extern float survey_west, survey_east, survey_north, survey_south;
extern float nav_radius;
extern float nav_ground_speed_pgain, nav_ground_speed_setpoint;
void nav_update(void);
void nav_home(void);
@@ -188,6 +190,15 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
nav_throttle_setpoint = _throttle; \
}
#define NavHeading(_course) { \
lateral_mode = LATERAL_MODE_COURSE; \
h_ctl_course_setpoint = _course; \
}
#define NavAttitude(_roll) { \
lateral_mode = LATERAL_MODE_ROLL; \
h_ctl_roll_setpoint = _roll; \
}
#endif /* NAV_H */
sw/airborne/traffic_info.c
+1 -1
View File
@@ -31,6 +31,6 @@
struct ac_info_ * get_ac_info(uint8_t id) {
id = (id < NB_ACS ? id : 0);
id = id < NB_ACS ? id : 0;
return &the_acs[id];
}
sw/airborne/traffic_info.h
-1
View File
@@ -49,7 +49,6 @@ struct ac_info_ the_acs[NB_ACS];
the_acs[_id].course = _course; \
the_acs[_id].alt = _alt; \
the_acs[_id].gspeed = _gspeed; \
/*** printf("%d:x=%.0f y=%.0f c=%f a=%.0f\n",_id,the_acs[_id].east,the_acs[_id].north,the_acs[_id].course, the_acs[_id].alt); ***/ \
} \
}
sw/logalizer/plot_roll_loop.c
+1 -1
View File
@@ -35,7 +35,7 @@ GtkWidget* build_gui ( void ) {
void on_DESIRED(IvyClientPtr app, void *user_data, int argc, char *argv[]){
float p = atof(argv[0]);
// float p = atof(argv[0]);
float phi = atof(argv[1]);
float phi_sp = atof(argv[2]);
sw/tools/gen_flight_plan.ml
+3 -4
View File
@@ -113,7 +113,7 @@ let get_index_block = fun x ->
let print_exception = fun x ->
let i = get_index_block (ExtXml.attrib x "deroute") in
let c = parsed_attrib x "cond" in
lprintf "if ((nav_block!=%s) && %s) { GotoBlock(%s) }\n" i c i
lprintf "if (%s && (nav_block != %s)) { GotoBlock(%s) }\n" c i i
let goto l = Xml.Element ("goto", ["name",l], [])
let exit_block = Xml.Element ("exit_block", [], [])
@@ -301,7 +301,7 @@ let rec print_stage = fun index_of_waypoints x ->
let until = parsed_attrib x "until" in
lprintf "if (%s) NextStage() else {\n" until;
right ();
lprintf "h_ctl_course_setpoint = RadOfDeg(%s);\n" (parsed_attrib x "course");
lprintf "NavHeading(RadOfDeg(%s));\n" (parsed_attrib x "course");
ignore (output_vmode x "" "");
output_cam_mode x index_of_waypoints;
left (); lprintf "}\n";
@@ -324,8 +324,7 @@ let rec print_stage = fun index_of_waypoints x ->
lprintf "{\n"
end;
right ();
lprintf "lateral_mode = LATERAL_MODE_ROLL;\n";
lprintf "h_ctl_roll_setpoint = RadOfDeg(%s);\n" (parsed_attrib x "roll");
lprintf "NavAttitude(RadOfDeg(%s));\n" (parsed_attrib x "roll");
ignore (output_vmode x "" "");
output_cam_mode x index_of_waypoints;
left (); lprintf "}\n";