diff --git a/conf/airframes/tl.xml b/conf/airframes/tl.xml index a67660665e..689abea6cd 100644 --- a/conf/airframes/tl.xml +++ b/conf/airframes/tl.xml @@ -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 diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index e865fe0a88..33b9da16e7 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -17,7 +17,7 @@ - + @@ -33,6 +33,7 @@ + @@ -150,6 +151,8 @@ cam_ac_target CDATA #IMPLIED target CDATA #IMPLIED radius CDATA #IMPLIED> + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/tl.xml b/conf/settings/tl.xml index c69d88b5bd..c6a1150e85 100644 --- a/conf/settings/tl.xml +++ b/conf/settings/tl.xml @@ -4,6 +4,7 @@ + diff --git a/conf/telemetry/tl.xml b/conf/telemetry/tl.xml index ef47927065..c8bc28810d 100644 --- a/conf/telemetry/tl.xml +++ b/conf/telemetry/tl.xml @@ -5,10 +5,12 @@ - - - + + + + + diff --git a/sw/airborne/common_nav.c b/sw/airborne/common_nav.c new file mode 100644 index 0000000000..3e789c7a8a --- /dev/null +++ b/sw/airborne/common_nav.c @@ -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(); +} diff --git a/sw/airborne/common_nav.h b/sw/airborne/common_nav.h new file mode 100644 index 0000000000..460af929c3 --- /dev/null +++ b/sw/airborne/common_nav.h @@ -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 */ diff --git a/sw/airborne/gps.c b/sw/airborne/gps.c index e2457e059b..d0c10dabae 100644 --- a/sw/airborne/gps.c +++ b/sw/airborne/gps.c @@ -32,6 +32,7 @@ #include "gps.h" #include "latlong.h" #include "sys_time.h" +#include "airframe.h" /** For backward compatibility */ #ifndef DOWNLINK_GPS_DEVICE diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c index f7eb4883b1..1aa60dfa53 100644 --- a/sw/airborne/gps_ubx.c +++ b/sw/airborne/gps_ubx.c @@ -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; diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index 9913638d4a..3698f4a982 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -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 diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index a2552719c8..93e3d71fe2 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -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) \ diff --git a/sw/airborne/tl_control.c b/sw/airborne/tl_control.c index cee7db7045..82c3bb174f 100644 --- a/sw/airborne/tl_control.c +++ b/sw/airborne/tl_control.c @@ -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)); } diff --git a/sw/airborne/tl_control.h b/sw/airborne/tl_control.h index c1f2266a09..a50b569be6 100644 --- a/sw/airborne/tl_control.h +++ b/sw/airborne/tl_control.h @@ -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; diff --git a/sw/airborne/tl_estimator.c b/sw/airborne/tl_estimator.c index 4d34e2224c..147b995d33 100644 --- a/sw/airborne/tl_estimator.c +++ b/sw/airborne/tl_estimator.c @@ -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.; diff --git a/sw/airborne/tl_estimator.h b/sw/airborne/tl_estimator.h index 09e8c98a0e..2066267f1f 100644 --- a/sw/airborne/tl_estimator.h +++ b/sw/airborne/tl_estimator.h @@ -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 */ diff --git a/sw/airborne/tl_main.c b/sw/airborne/tl_main.c index 2eb5692668..2707d3a5f3 100644 --- a/sw/airborne/tl_main.c +++ b/sw/airborne/tl_main.c @@ -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(); diff --git a/sw/airborne/tl_nav.c b/sw/airborne/tl_nav.c new file mode 100644 index 0000000000..ceb4dcc44c --- /dev/null +++ b/sw/airborne/tl_nav.c @@ -0,0 +1,34 @@ +#include + +#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; +} + + + + diff --git a/sw/airborne/tl_nav.h b/sw/airborne/tl_nav.h new file mode 100644 index 0000000000..19f97f9324 --- /dev/null +++ b/sw/airborne/tl_nav.h @@ -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 diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 49ac171dfe..cb391ea893 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -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 "" +let home_block = Xml.parse_string "" 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