Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 04:13:39 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

airspeed estimation added in WIND_INFO message

Browse Source
This commit is contained in:
Pascal Brisset
2008-01-03 13:30:26 +00:00
parent ec35ed4512
commit b5f37873e8
9 changed files with 58 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/snav.xml
+3 -2
View File
@@ -43,11 +43,12 @@
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="Smooth nav">
<call fun="snav_init(WP_1, M_PI_2-atan2(WaypointY(WP_2)-WaypointY(WP_1),WaypointX(WP_2)-WaypointX(WP_1)), gps_itow / 1000. + 200.)"/>
<set var="snav_desired_tow" value="gps_itow / 1000. + 200."/>
<call fun="snav_init(WP_1, M_PI_2-atan2(WaypointY(WP_2)-WaypointY(WP_1),WaypointX(WP_2)-WaypointX(WP_1)), DEFAULT_CIRCLE_RADIUS/2.)"/>
<call fun="snav_circle1()"/>
<call fun="snav_route()"/>
<call fun="snav_circle2()"/>
<call fun="snav_on_time()"/>
<call fun="snav_on_time(DEFAULT_CIRCLE_RADIUS)"/>
<go from="1" hmode="route" wp="2"/>
<deroute block="Standby"/>
</block>
conf/messages.xml
+1
View File
@@ -614,6 +614,7 @@
<field name="pad0" type="uint8"/>
<field name="east" type="float" unit="m/s"/>
<field name="north" type="float" unit="m/s"/>
<field name="airspeed" type="float" unit="m/s"/>
</message>
<message name="SETTING" ID="4">
sw/airborne/datalink.c
+1
View File
@@ -86,6 +86,7 @@ void dl_parse_msg(void) {
} else if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
wind_east = DL_WIND_INFO_east(dl_buffer);
wind_north = DL_WIND_INFO_north(dl_buffer);
estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
}
#endif /** NAV */
#ifdef HITL
sw/airborne/estimator.c
+1 -1
View File
@@ -61,7 +61,7 @@ float estimator_hspeed_dir;
/* wind */
float wind_east, wind_north;
float estimator_airspeed;
#define NORM_RAD_ANGLE2(x) { \
while (x > 2 * M_PI) x -= 2 * M_PI; \
sw/airborne/estimator.h
+3 -2
View File
@@ -62,8 +62,9 @@ extern float estimator_t;
extern float estimator_hspeed_mod;
extern float estimator_hspeed_dir;
/* wind */
extern float wind_east, wind_north;
/* Wind and airspeed estimatation sent by the GPS */
extern float wind_east, wind_north; /* m/s */
extern float estimator_airspeed; /* m/s */
void estimator_init( void );
sw/airborne/snav.c
+37 -30
View File
@@ -14,11 +14,12 @@ static float d_radius, a_radius;
static float qdr_td, qdr_a;
static uint8_t wp_a;
float snav_desired_tow; /* time of week, s */
static float u_a_ca_x, u_a_ca_y;
/* D is the current position */
bool_t snav_init(uint8_t a, float desired_course_rad, float tow) {
bool_t snav_init(uint8_t a, float desired_course_rad, float radius) {
wp_a = a;
snav_desired_tow = tow;
radius = fabs(radius);
float da_x = WaypointX(wp_a) - estimator_x;
float da_y = WaypointY(wp_a) - estimator_y;
@@ -26,7 +27,7 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float tow) {
/* D_CD orthogonal to current course, CD on the side of A */
float u_x = cos(M_PI_2 - estimator_hspeed_dir);
float u_y = sin(M_PI_2 - estimator_hspeed_dir);
d_radius = - Sign(u_x*da_y - u_y*da_x) * DEFAULT_CIRCLE_RADIUS;
d_radius = - Sign(u_x*da_y - u_y*da_x) * radius;
wp_cd.x = estimator_x + d_radius * u_y;
wp_cd.y = estimator_y - d_radius * u_x;
wp_cd.a = WaypointAlt(wp_a);
@@ -34,9 +35,11 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float tow) {
/* A_CA orthogonal to desired course, CA on the side of D */
float desired_u_x = cos(M_PI_2 - desired_course_rad);
float desired_u_y = sin(M_PI_2 - desired_course_rad);
a_radius = Sign(desired_u_x*da_y - desired_u_y*da_x) * DEFAULT_CIRCLE_RADIUS;
wp_ca.x = WaypointX(wp_a) + a_radius * desired_u_y;
wp_ca.y = WaypointY(wp_a) - a_radius * desired_u_x;
a_radius = Sign(desired_u_x*da_y - desired_u_y*da_x) * radius;
u_a_ca_x = desired_u_y;
u_a_ca_y = - desired_u_x;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
wp_ca.a = WaypointAlt(wp_a);
/* Unit vector along CD-CA */
@@ -45,24 +48,24 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float tow) {
float cd_ca = sqrt(u_x*u_x+u_y*u_y);
/* If it is too close in reverse direction, set CA on the other side */
if (a_radius * d_radius < 0 && cd_ca < 2 * DEFAULT_CIRCLE_RADIUS) {
if (a_radius * d_radius < 0 && cd_ca < 2 * radius) {
a_radius = -a_radius;
wp_ca.x = WaypointX(wp_a) + a_radius * desired_u_y;
wp_ca.y = WaypointY(wp_a) - a_radius * desired_u_x;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
u_x = wp_ca.x - wp_cd.x;
u_y = wp_ca.y - wp_cd.y;
cd_ca = sqrt(u_x*u_x+u_y*u_y);
}
u_x /= cd_ca;
u_y /= cd_ca;
if (a_radius * d_radius > 0) {
/* Both arcs are in the same direction */
/* CD_TD orthogonal to CD_CA */
wp_td.x = wp_cd.x - d_radius * u_y;
wp_td.y = wp_cd.y + d_radius * u_x;
/* CA_TA also orthogonal to CD_CA */
wp_ta.x = wp_ca.x - a_radius * u_y;
wp_ta.y = wp_ca.y + a_radius * u_x;
@@ -71,7 +74,7 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float tow) {
float alpha = atan2(u_y, u_x) + acos(d_radius/(cd_ca/2));
wp_td.x = wp_cd.x + d_radius * cos(alpha);
wp_td.y = wp_cd.y + d_radius * sin(alpha);
wp_ta.x = wp_ca.x + a_radius * cos(alpha);
wp_ta.y = wp_ca.y + a_radius * sin(alpha);
}
@@ -79,11 +82,11 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float tow) {
qdr_a = M_PI_2 - atan2(WaypointY(wp_a)-wp_ca.y, WaypointX(wp_a)-wp_ca.x);
wp_td.a = wp_cd.a;
wp_ta.a = wp_ca.a;
return FALSE;
}
bool_t snav_circle1(void) {
/* circle around CD until QDR_TD */
NavVerticalAutoThrottleMode(0); /* No pitch */
@@ -110,31 +113,35 @@ bool_t snav_circle2(void) {
return(! NavQdrCloseTo(DegOfRad(qdr_a)));
}
bool_t snav_on_time(void) {
static float radius_factor = 1.;
/* Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
bool_t snav_on_time(float nominal_radius) {
nominal_radius = fabs(nominal_radius);
/* circle tangent to A */
struct point current_center;
current_center.x = WaypointX(wp_a) + (wp_ca.x - WaypointX(wp_a)) * radius_factor;
current_center.y = WaypointY(wp_a) + (wp_ca.y - WaypointY(wp_a)) * radius_factor;
float current_qdr = M_PI_2 - atan2(estimator_y-current_center.y, estimator_x-current_center.x);
float current_qdr = M_PI_2 - atan2(estimator_y-wp_ca.y, estimator_x-wp_ca.x);
float remaining_angle = Sign(a_radius)*(qdr_a - current_qdr);
if (remaining_angle < 0.)
if (remaining_angle <= 0.)
remaining_angle += 2 * M_PI;
float remaining_time = snav_desired_tow - gps_itow / 1000.;
radius_factor = (remaining_time * NOMINAL_AIRSPEED) / (remaining_angle * DEFAULT_CIRCLE_RADIUS);
/* Use the nominal airspeed if the estimated on is not realistic */
float airspeed = estimator_airspeed;
if (estimator_airspeed < NOMINAL_AIRSPEED / 2. ||
estimator_airspeed > 2.* NOMINAL_AIRSPEED)
estimator_airspeed = NOMINAL_AIRSPEED;
/* printf("ra=%.0f rt=%.0f\n", DegOfRad(remaining_angle), remaining_time); */
if (radius_factor > 2.)
radius_factor = 1.;
/* Radius size to finish in one single circle */
float radius = (remaining_time * airspeed) / remaining_angle;
if (radius > 2. * nominal_radius)
radius = nominal_radius;
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
nav_circle_XY(current_center.x, current_center.y, a_radius * radius_factor);
radius *= Sign(a_radius);
wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
nav_circle_XY(wp_ca.x, wp_ca.y, radius);
/* Stay in this mode until the end of time */
return(remaining_time > 0);
}
sw/airborne/snav.h
+2 -2
View File
@@ -5,11 +5,11 @@
extern float snav_desired_tow; /* time of week, s */
bool_t snav_init(uint8_t wp_a, float desired_course_rad, float desired_tow);
bool_t snav_init(uint8_t wp_a, float desired_course_rad, float radius);
bool_t snav_circle1(void);
bool_t snav_route(void);
bool_t snav_circle2(void);
bool_t snav_on_time(void);
bool_t snav_on_time(float radius);
#endif // SNAV_H
sw/ground_segment/cockpit/live.ml
+8 -4
View File
@@ -94,7 +94,8 @@ type aircraft = {
mutable got_track_status_timer : int;
mutable last_dist_to_wp : float;
mutable dl_values : float array;
mutable last_unix_time : float
mutable last_unix_time : float;
mutable airspeed : float
}
let aircrafts = Hashtbl.create 3
@@ -506,6 +507,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
notebook_label = _label;
got_track_status_timer = 1000;
dl_values = [||]; last_unix_time = 0.;
airspeed = 0.
} in
Hashtbl.add aircrafts ac_id ac;
select_ac acs_notebook ac_id;
@@ -519,10 +521,11 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id
let a = (pi/.2. -. ac.wind_dir)
and w = ac.wind_speed in
let wind_east = (sprintf "%.1f" (-. cos a *. w))
and wind_north = (sprintf "%.1f" (-. sin a *. w)) in
let wind_east = sprintf "%.1f" (-. cos a *. w)
and wind_north = sprintf "%.1f" (-. sin a *. w)
and airspeed = sprintf "%.1f" ac.airspeed in
let msg_items = ["WIND_INFO"; ac_id; "42"; wind_east; wind_north] in
let msg_items = ["WIND_INFO"; ac_id; "42"; wind_east; wind_north;airspeed] in
let value = String.concat ";" msg_items in
let vs = ["ac_id", Pprz.String ac_id; "message", Pprz.String value] in
Ground_Pprz.message_send "dl" "RAW_DATALINK" vs;
@@ -626,6 +629,7 @@ let get_wind_msg = fun (geomap:G.widget) _sender vs ->
let ac = get_ac vs in
let value = fun field_name -> Pprz.float_assoc field_name vs in
let airspeed = value "mean_aspeed" in
ac.airspeed <- airspeed;
ac.strip#set_airspeed airspeed;
ac.misc_page#set_mean_aspeed (sprintf "%.1f" airspeed);
ac.wind_speed <- value "wspeed";
sw/ground_segment/cockpit/live.mli
+2 -1
View File
@@ -58,7 +58,8 @@ type aircraft = {
mutable got_track_status_timer : int;
mutable last_dist_to_wp : float;
mutable dl_values : float array;
mutable last_unix_time : float
mutable last_unix_time : float;
mutable airspeed : float
}
val aircrafts : (string, aircraft) Hashtbl.t