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

nav for tl

Browse Source
This commit is contained in:
Pascal Brisset
2008-01-28 02:29:12 +00:00
parent 759c6a87fa
commit a1dffbffe1
19 changed files with 252 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tl.xml
+1 -1
View File
@@ -75,7 +75,7 @@ ap.srcs += tl_autopilot.c tl_control.c tl_estimator.c
ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 -DDOWNLINK_GPS_DEVICE=DOWNLINK_AP_DEVICE -DGPS_CONFIGURE -DUSER_GPS_CONFIGURE=\"tl_gps_configure.h\" -DGPS_BAUD=38400
ap.srcs += gps_ubx.c gps.c latlong.c
ap.srcs += common_nav.c tl_nav.c
</makefile>
conf/flight_plans/flight_plan.dtd
+4 -1
View File
@@ -17,7 +17,7 @@
<!ELEMENT exceptions (exception*)>
<!ELEMENT blocks (block+)>
<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval)*>
<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|home)*>
<!ELEMENT include (arg|with)*>
<!ELEMENT arg EMPTY>
@@ -33,6 +33,7 @@
<!ELEMENT set EMPTY>
<!ELEMENT call EMPTY>
<!ELEMENT circle EMPTY>
<!ELEMENT home EMPTY>
<!ELEMENT eight EMPTY>
<!ELEMENT oval EMPTY>
<!ELEMENT survey_rectangle EMPTY>
@@ -150,6 +151,8 @@ cam_ac_target CDATA #IMPLIED
target CDATA #IMPLIED
radius CDATA #IMPLIED>
<!ATTLIST home>
<!ATTLIST circle
wp CDATA #REQUIRED
wp_qdr CDATA #IMPLIED
conf/flight_plans/tl.xml
+27
View File
@@ -0,0 +1,27 @@
<flight_plan name="booz test 1" lon0="1.27289" lat0="43.46223" ground_alt="0" alt="3" max_dist_from_home="400" security_height="1">
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="TARGET" x="10" y="0"/>
</waypoints>
<blocks>
<block name="Wait GPS">
<set value="1" var="kill_throttle"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<set value="1" var="kill_throttle"/>
<stay wp="HOME"/>
</block>
<block name="target">
<stay wp="TARGET"/>
</block>
</blocks>
</flight_plan>
conf/settings/tl.xml
+1
View File
@@ -4,6 +4,7 @@
<dl_settings>
<dl_settings NAME="flight params">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="estimator_in_flight" shortname="in_flight" module="tl_estimator"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle" shortname="in_flight" module="tl_control"/>
</dl_settings>
</dl_settings>
</settings>
conf/telemetry/tl.xml
+5 -3
View File
@@ -5,10 +5,12 @@
<process name="Ap">
<mode name="default">
<message name="PPM" period="1"/>
<message name="RC" period="1"/>
<message name="COMMANDS" period="1"/>
<message name="PPM" period="5"/>
<message name="RC" period="5"/>
<message name="COMMANDS" period="5"/>
<message name="ACTUATORS" period="1"/> <!-- For trimming -->
<message name="BAT" period="3"/>
<message name="ESTIMATOR" period="1"/>
</mode>
</process>
sw/airborne/common_nav.c
+73
View File
@@ -0,0 +1,73 @@
#include "common_nav.h"
#include "estimator.h"
#include "flight_plan.h"
#include "gps.h"
float dist2_to_home;
bool_t too_far_from_home;
const uint8_t nb_waypoint;
struct point waypoints[NB_WAYPOINT] = WAYPOINTS;
uint8_t nav_stage, nav_block;
uint16_t stage_time, block_time;
float ground_alt;
int32_t nav_utm_east0 = NAV_UTM_EAST0;
int32_t nav_utm_north0 = NAV_UTM_NORTH0;
uint8_t nav_utm_zone0 = NAV_UTM_ZONE0;
/** \brief Computes square distance to the HOME waypoint potentially sets
* \a too_far_from_home
*/
void compute_dist2_to_home(void) {
float ph_x = waypoints[WP_HOME].x - estimator_x;
float ph_y = waypoints[WP_HOME].y - estimator_y;
dist2_to_home = ph_x*ph_x + ph_y *ph_y;
too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME);
#if defined InAirspace
too_far_from_home = too_far_from_home || !(InAirspace(estimator_x, estimator_y));
#endif
}
static float previous_ground_alt;
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
#ifdef GPS_USE_LATLONG
/* Set the real UTM zone */
nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
/* Recompute UTM coordinates in this zone */
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
nav_utm_east0 = latlong_utm_x;
nav_utm_north0 = latlong_utm_y;
#else
nav_utm_zone0 = gps_utm_zone;
nav_utm_east0 = gps_utm_east/100;
nav_utm_north0 = gps_utm_north/100;
#endif
previous_ground_alt = ground_alt;
ground_alt = gps_alt/100;
return 0;
}
/** Shift altitude of the waypoint according to a new ground altitude */
unit_t nav_update_waypoints_alt( void ) {
uint8_t i;
for(i = 0; i < NB_WAYPOINT; i++) {
waypoints[i].a += ground_alt - previous_ground_alt;
}
return 0;
}
void nav_init_block(void) {
if (nav_block >= NB_BLOCK)
nav_block=NB_BLOCK-1;
nav_stage = 0;
block_time = 0;
InitStage();
}
sw/airborne/common_nav.h
+66
View File
@@ -0,0 +1,66 @@
#ifndef COMMON_NAV_H
#define COMMON_NAV_H
#include "std.h"
extern float dist2_to_home;
extern bool_t too_far_from_home;
struct point {
float x;
float y;
float a;
};
#define WaypointX(_wp) (waypoints[_wp].x)
#define WaypointY(_wp) (waypoints[_wp].y)
#define WaypointAlt(_wp) (waypoints[_wp].a)
extern const uint8_t nb_waypoint;
extern struct point waypoints[];
/** size == nb_waypoint, waypoint 0 is a dummy waypoint */
/** In s */
extern uint16_t stage_time, block_time;
extern uint8_t nav_stage, nav_block;
extern float ground_alt; /* m */
extern int32_t nav_utm_east0; /* m */
extern int32_t nav_utm_north0; /* m */
extern uint8_t nav_utm_zone0;
void nav_init_stage( void );
void nav_init_block(void);
void compute_dist2_to_home(void);
unit_t nav_reset_reference( void ) __attribute__ ((unused));
unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused));
#define InitStage() { nav_init_stage(); return; }
#define Block(x) case x: nav_block=x;
#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); }
#define NextBlock() { nav_block++; nav_init_block(); return; }
#define GotoBlock(b) { nav_block=b; nav_init_block(); return; }
#define Stage(s) case s: nav_stage=s;
#define NextStage() { nav_stage++; InitStage() }
#define NextStageFrom(wp) { last_wp = wp; NextStage() }
#define GotoStage(s) { nav_stage = s; InitStage() }
#define Label(x) label_ ## x:
#define Goto(x) { goto label_ ## x; }
#define And(x, y) ((x) && (y))
#define Or(x, y) ((x) || (y))
#define Min(x,y) (x < y ? x : y)
#define Max(x,y) (x > y ? x : y)
#define NavBlockTime() (block_time)
#define LessThan(_x, _y) ((_x) < (_y))
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
#endif /* COMMON_NAV_H */
sw/airborne/gps.c
+1
View File
@@ -32,6 +32,7 @@
#include "gps.h"
#include "latlong.h"
#include "sys_time.h"
#include "airframe.h"
/** For backward compatibility */
#ifndef DOWNLINK_GPS_DEVICE
sw/airborne/gps_ubx.c
+2
View File
@@ -104,7 +104,9 @@ static uint8_t ck_a, ck_b;
uint8_t send_ck_a, send_ck_b;
bool_t gps_configuring;
#ifdef GPS_CONFIGURE
static uint8_t gps_status_config;
#endif
void gps_init( void ) {
ubx_status = UNINIT;
sw/airborne/nav.c
+3 -96
View File
@@ -43,8 +43,6 @@
#define RCLost() bit_is_set(fbw_state->status, RADIO_REALLY_LOST)
uint8_t nav_stage, nav_block;
/** To save the current block/stage to enable return */
static uint8_t last_block, last_stage;
@@ -54,7 +52,6 @@ float last_x, last_y;
static uint8_t last_wp __attribute__ ((unused));
float rc_pitch;
uint16_t stage_time, block_time;
float stage_time_ds;
float carrot_x, carrot_y;
@@ -111,20 +108,10 @@ void nav_init_stage( void ) {
#define RcEvent1() CheckEvent(rc_event_1)
#define RcEvent2() CheckEvent(rc_event_2)
#define Block(x) case x: nav_block=x;
void init_block(void);
#define NextBlock() { nav_block++; init_block(); return; }
#define GotoBlock(b) { nav_block=b; init_block(); return; }
#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0; return; FALSE;})
#define Stage(s) case s: nav_stage=s;
#define NextStage() { nav_stage++; InitStage() }
#define NextStageFrom(wp) { last_wp = wp; NextStage() }
#define GotoStage(s) { nav_stage = s; InitStage() }
#define Label(x) label_ ## x:
#define Goto(x) { goto label_ ## x; }
static inline void fly_to_xy(float x, float y);
@@ -217,20 +204,11 @@ void nav_circle_XY(float x, float y, float radius) {
nav_circle_XY(carrot_x, carrot_y, radius); \
}
#define And(x, y) ((x) && (y))
#define Or(x, y) ((x) || (y))
#define Min(x,y) (x < y ? x : y)
#define Max(x,y) (x > y ? x : y)
#define NavBlockTime() (block_time)
#define LessThan(_x, _y) ((_x) < (_y))
#define NavFollow(_ac_id, _distance, _height) \
nav_follow(_ac_id, _distance, _height);
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
void nav_goto_block(uint8_t b) {
if (b != nav_block) { /* To avoid a loop in a the current block */
last_block = nav_block;
@@ -242,9 +220,6 @@ void nav_goto_block(uint8_t b) {
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);
#ifdef NAV_GROUND_SPEED_PGAIN
@@ -298,18 +273,6 @@ static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float g
#include "flight_plan.h"
void init_block(void) {
if (nav_block >= NB_BLOCK)
nav_block=NB_BLOCK-1;
nav_stage = 0;
block_time = 0;
InitStage();
}
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.);
@@ -326,55 +289,12 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
#endif
}
/** Reset the geographic reference to the current GPS fix */
static unit_t nav_reset_reference( void ) {
#ifdef GPS_USE_LATLONG
/* Set the real UTM zone */
nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
/* Recompute UTM coordinates in this zone */
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
nav_utm_east0 = latlong_utm_x;
nav_utm_north0 = latlong_utm_y;
#else
nav_utm_zone0 = gps_utm_zone;
nav_utm_east0 = gps_utm_east/100;
nav_utm_north0 = gps_utm_north/100;
#endif
previous_ground_alt = ground_alt;
ground_alt = gps_alt/100;
return 0;
}
/** Shift altitude of the waypoint according to a new ground altitude */
static unit_t nav_update_waypoints_alt( void ) {
uint8_t i;
for(i = 0; i < NB_WAYPOINT; i++) {
waypoints[i].a += ground_alt - previous_ground_alt;
}
return 0;
}
int32_t nav_utm_east0 = NAV_UTM_EAST0;
int32_t nav_utm_north0 = NAV_UTM_NORTH0;
uint8_t nav_utm_zone0 = NAV_UTM_ZONE0;
float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
float desired_x, desired_y;
pprz_t nav_throttle_setpoint;
float nav_pitch;
float dist2_to_wp, dist2_to_home;
bool_t too_far_from_home;
const uint8_t nb_waypoint = NB_WAYPOINT;
struct point waypoints[NB_WAYPOINT] = WAYPOINTS;
float dist2_to_wp;
/** \brief Decide if the UAV is approaching the current waypoint.
* Computes \a dist2_to_wp and compare it to square \a carrot.
@@ -434,20 +354,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
fly_to_xy(last_wp_x + nav_leg_progress*leg_x +nav_shift*leg_y/nav_leg_length, last_wp_y + nav_leg_progress*leg_y-nav_shift*leg_x/nav_leg_length);
}
/** \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;
float ph_y = waypoints[WP_HOME].y - estimator_y;
dist2_to_home = ph_x*ph_x + ph_y *ph_y;
too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME);
#if defined InAirspace
too_far_from_home = too_far_from_home || !(InAirspace(estimator_x, estimator_y));
#endif
}
#include "common_nav.c"
#ifndef FAILSAFE_HOME_RADIUS
#define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
sw/airborne/nav.h
+2 -29
View File
@@ -37,43 +37,22 @@
#include "airframe.h"
#include "fw_v_ctl.h"
#include "nav_survey_rectangle.h"
#include "common_nav.h"
#define G 9.806
#define Square(_x) ((_x)*(_x))
#define DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y))
struct point {
float x;
float y;
float a;
};
#define WaypointX(_wp) (waypoints[_wp].x)
#define WaypointY(_wp) (waypoints[_wp].y)
#define WaypointAlt(_wp) (waypoints[_wp].a)
extern float cur_pos_x;
extern float cur_pos_y;
extern float last_x, last_y;
extern uint8_t nav_stage, nav_block;
extern float dist2_to_wp, dist2_to_home;
extern float dist2_to_wp;
extern int32_t nav_utm_east0; /* m */
extern int32_t nav_utm_north0; /* m */
extern uint8_t nav_utm_zone0;
extern const uint8_t nb_waypoint;
extern struct point waypoints[];
/** size == nb_waypoint, waypoint 0 is a dummy waypoint */
extern float desired_x, desired_y, nav_altitude, flight_altitude, nav_glide_pitch_trim;
extern pprz_t nav_throttle_setpoint;
extern float nav_pitch, rc_pitch;
extern bool_t too_far_from_home;
/** in second */
extern uint16_t stage_time, block_time;
/** in second */
extern float stage_time_ds;
@@ -110,8 +89,6 @@ extern void nav_oval_init( void );
extern void nav_oval(uint8_t, uint8_t, float);
#define Oval(a, b, c) nav_oval((b), (a), (c))
extern float ground_alt;
extern float nav_radius; /* m */
extern float nav_course; /* degrees, clockwise, 0.0 = N */
extern float nav_climb; /* m/s */
@@ -153,10 +130,6 @@ extern void nav_circle_XY(float x, float y, float radius);
#define NavQdrCloseTo(x) ({ float _course = x; NormCourse(_course); float circle_qdr = NavCircleQdr(); (Min(_course, 350) < circle_qdr && circle_qdr < _course+10); })
#define NavCourseCloseTo(x) ({ float _course = x; NormCourse(_course); float deg = DegOfRad(estimator_hspeed_dir); (Min(_course, 350) < deg && deg < _course+10); })
extern void nav_init_stage( void );
#define InitStage() { nav_init_stage(); return; }
/*********** Navigation along a line *************************************/
extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y);
#define NavSegment(_start, _end) \
sw/airborne/tl_control.c
+4 -1
View File
@@ -3,6 +3,9 @@
#include "tl_estimator.h"
#include "radio_control.h"
bool_t kill_throttle; /* keep this old name to get it in the GCS */
// output
float tl_control_p_sp;
float tl_control_q_sp;
@@ -90,7 +93,7 @@ void tl_control_rate_run(void) {
tl_control_commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)cmd_p);
tl_control_commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)cmd_q);
tl_control_commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)cmd_r);
tl_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (tl_control_power_sp));
tl_control_commands[COMMAND_THROTTLE] = kill_throttle ? 0 : TRIM_PPRZ((int16_t) (tl_control_power_sp));
}
sw/airborne/tl_control.h
+2
View File
@@ -12,6 +12,8 @@ extern void tl_control_rate_run(void);
extern void tl_control_attitude_read_setpoints_from_rc(void);
extern void tl_control_attitude_run(void);
extern bool_t kill_throttle;
extern float tl_control_p_sp;
extern float tl_control_q_sp;
extern float tl_control_r_sp;
sw/airborne/tl_estimator.c
+4 -4
View File
@@ -5,8 +5,8 @@
bool_t estimator_in_flight;
uint16_t estimator_flight_time;
float estimator_east; /* m */
float estimator_north; /* m */
float estimator_x; /* m */
float estimator_y; /* m */
float estimator_z; /* altitude in m */
float estimator_speed; /* m/s */
@@ -21,8 +21,8 @@ void tl_estimator_use_gps(void) {
float gps_north = gps_utm_north / 100.;
/* Relative position to reference */
estimator_east = gps_east - NAV_UTM_EAST0;
estimator_north = gps_north - NAV_UTM_NORTH0;
estimator_x = gps_east - NAV_UTM_EAST0;
estimator_y = gps_north - NAV_UTM_NORTH0;
estimator_z = gps_alt / 100.;
estimator_speed = gps_gspeed / 100.;
sw/airborne/tl_estimator.h
+2 -2
View File
@@ -6,8 +6,8 @@
extern bool_t estimator_in_flight;
extern uint16_t estimator_flight_time;
extern float estimator_east; /* m */
extern float estimator_north; /* m */
extern float estimator_x; /* m */
extern float estimator_y; /* m */
extern float estimator_z; /* altitude in m */
extern float estimator_speed; /* m/s */
sw/airborne/tl_main.c
+3
View File
@@ -18,6 +18,7 @@
#include "adc.h"
#include "tl_bat.h"
#include "gps.h"
#include "tl_nav.h"
static inline void tl_main_init( void );
static inline void tl_main_periodic_task( void );
@@ -65,6 +66,8 @@ static inline void tl_main_periodic_task( void ) {
radio_control_periodic_task();
if (rc_status != RC_OK)
tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
RunOnceEvery(15, tl_nav_periodic_task());
/* run control loops */
tl_autopilot_periodic_task();
sw/airborne/tl_nav.c
+34
View File
@@ -0,0 +1,34 @@
#include <math.h>
#include "tl_nav.h"
#include "tl_estimator.h"
#include "tl_control.h"
#include "gps.h"
#define NavGotoWaypoint(_wp) {}
#define NavVerticalAutoThrottleMode(_foo) {}
#define NavVerticalAltitudeMode(_foo, _bar) {}
static void nav_home(void) {
}
#define NAV_C
#include "flight_plan.h"
void tl_nav_init(void) {
nav_block = 0;
nav_stage = 0;
}
void tl_nav_periodic_task(void) {
compute_dist2_to_home();
auto_nav();
}
void nav_init_stage( void ) {
stage_time = 0;
}
sw/airborne/tl_nav.h
+12
View File
@@ -0,0 +1,12 @@
#ifndef TL_NAV_H
#define TL_NAV_H
#include "common_nav.h"
void tl_nav_init(void);
/** To be called at 4Hz */
void tl_nav_periodic_task(void);
#endif // TL_NAV_H
sw/tools/gen_flight_plan.ml
+6 -2
View File
@@ -122,7 +122,7 @@ let print_exception = fun x ->
let element = fun a b c -> Xml.Element (a, b, c)
let goto l = element "goto" ["name",l] []
let exit_block = element "exit_block" [] []
let home_block = Xml.parse_string "<block name=\"circle HOME\"><circle wp=\"HOME\" radius=\"DEFAULT_CIRCLE_RADIUS\"/></block>"
let home_block = Xml.parse_string "<block name=\"HOME\"><home/></block>"
let stage = ref 0
@@ -245,7 +245,7 @@ let rec index_stage = fun x ->
let l = List.map index_stage (Xml.children x) in
incr stage; (* To count the loop stage *)
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi n], l)
| "return" | "goto" | "deroute" | "exit_block" | "follow" | "call"
| "return" | "goto" | "deroute" | "exit_block" | "follow" | "call" | "home"
| "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" ->
incr stage;
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
@@ -390,6 +390,10 @@ let rec print_stage = fun index_of_waypoints x ->
ignore (output_vmode x "" ""); (** To handle "pitch" *)
output_cam_mode x index_of_waypoints;
lprintf "return;\n"
| "home" ->
stage ();
lprintf "nav_home();\n";
lprintf "return;\n"
| "circle" ->
stage ();
let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in